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/.gitattributes b/.gitattributes new file mode 100644 index 0000000..912a0a0 --- /dev/null +++ b/.gitattributes @@ -0,0 +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/.gitignore b/.gitignore index 8c7227b..deeb09f 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,7 @@ tmp/ *.pth *.onnx *.tflite + +# Local offline experiment outputs +outputs/ +src/azas_bringup/config/*.bak-* diff --git a/COMMANDS.md b/COMMANDS.md index 6e8f490..7963f53 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 @@ -111,6 +117,153 @@ 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 + +`calibration.yaml`의 `cup_pre_place_joints_deg`, `cup_place_joints_deg`, +`press_pre_joints_deg`, `press_contact_joints_deg`를 읽어서 `/joint_states`만 +publish합니다. 실제 로봇 서비스, MoveJoint, MoveLine, 그리퍼 서비스는 호출하지 +않습니다. + +RViz까지 한 번에 띄우려면 이 wrapper를 씁니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +bash tools/run/show_measured_recipe_joint_preview_rviz.sh --dispenser-ids 1 --loop +``` + +이 wrapper는 기본적으로 창현 side-grip 로직과 같은 source tree의 +`src/azas_bringup/config/safety.yaml`, +`src/azas_bringup/config/measured_dispenser_collision.yaml`을 명시적으로 로드합니다. +`side_grip_table`, `side_grip_workspace_*` 안전영역, measured dispenser collision, +full collision YAML marker를 RViz에 띄웁니다. 깜빡임이 있으면 남은 preview 노드를 +정리하고 다시 실행합니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source install/setup.bash +pkill -f 'preview_measured_dispenser_recipe_rviz.py|publish_measured_recipe_joint_rviz_preview.py|joint_state_relay.py|dispenser_sequence_preview_node|workspace_collision_scene_node|measured_dispenser_collision_scene_node|collision_scene_rviz_publisher|link6_gripper_collision_node|__node:=m0609_robot_state_publisher' || true +pkill -x rviz2 || true + +DISPLAY=:20.0 \ +SHOW_WORKSPACE_SAFETY=true \ +SHOW_MEASURED_DISPENSER_COLLISION=true \ +SHOW_FULL_COLLISION_SCENE=true \ +PUBLISH_WORKSPACE_COLLISION_OBJECTS=true \ +PUBLISH_DISPENSER_COLLISION_OBJECTS=true \ +JOINT_VELOCITY_DEG_S=40.0 \ +HOLD_SECONDS=1.0 \ +PUBLISH_RATE=60.0 \ +bash tools/run/show_measured_recipe_joint_preview_rviz.sh \ + --dispenser-ids 1 \ + --no-press-reset-before-press \ + --press-use-recorded-pre-joints \ + --press-depth-m 0.040 \ + --press-extra-depth-m 0.0 \ + --safe-lift-joint-fallback \ + --press-lock-contact-joints 6 \ + --loop +``` + +첫 번째 터미널에서 RViz demo를 실행합니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +ros2 launch dsr_moveit_config_m0609 demo.launch.py +``` + +다른 터미널에서 측정 조인트 preview publisher를 실행합니다. 이 Python 명령만으로는 +RViz 창을 띄우지 않습니다. + +```bash +source /opt/ros/humble/setup.bash +cd /home/ssu/Azas +source install/setup.bash +python3 tools/run/preview_measured_dispenser_recipe_rviz.py --dispenser-ids 1 --loop +``` + +예: 1번 디스펜서를 2회, 3번 디스펜서를 1회 순서로 표시 + +```bash +python3 tools/run/preview_measured_dispenser_recipe_rviz.py --dispenser-ids 1x2,3 +``` + +### 색상 스캔 자세 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. 비-하드웨어 점검 @@ -302,6 +455,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. 스모크 테스트 하드웨어 없이 실행 가능한 자동화 테스트입니다. @@ -313,6 +548,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/DESIGN.md b/DESIGN.md new file mode 100644 index 0000000..8f4dd28 --- /dev/null +++ b/DESIGN.md @@ -0,0 +1,83 @@ +# Design + +## Source of truth +- Status: Draft +- 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/`. + +## 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, 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, 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. +- 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, vision feed and robot process status nearby, recipe catalog below. + +## 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, 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 +- 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. +- 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. +- 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 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 +- [ ] 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/README.md b/README.md index 5505bb1..4503c3a 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,24 @@ git pull origin develop bash tools/setup/bootstrap_local_workspace.sh ``` +다른 팀원 PC를 같은 상태로 맞출 때는 branch/build/package 검증까지 포함한 아래 스크립트를 사용합니다. + +```bash +bash tools/setup/bootstrap_team_pc.sh +``` + +YOLO 모델은 각 PC에서 repo-local 경로로 연결합니다. `best.pt` 파일 자체는 Git에 커밋하지 않습니다. + +```bash +bash tools/setup/link_yolo_model.sh /path/to/best.pt +``` + +RealSense가 패널에서 안 뜨는 PC는 먼저 아래 비-모션 체크로 카메라 패키지, USB 인식, ROS 토픽을 확인합니다. + +```bash +bash tools/checks/check_realsense_camera_ready.sh +``` + 패널 실행: ```bash diff --git a/asdfas b/asdfas new file mode 100644 index 0000000..5d57cf3 --- /dev/null +++ b/asdfas @@ -0,0 +1,129 @@ +diff --git a/tools/run/check_one_click_cocktail_config.sh b/tools/run/check_one_click_cocktail_config.sh +index 34ac73a..e45cfba 100755 +--- a/tools/run/check_one_click_cocktail_config.sh ++++ b/tools/run/check_one_click_cocktail_config.sh +@@ -89,6 +89,7 @@ try: + 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_pre_joints_deg", 6, f"dispenser_outlets.{dispenser_id}") + require_list(outlet, "press_contact_joints_deg", 6, f"dispenser_outlets.{dispenser_id}") +  + grouped: list[tuple[str, int]] = [] +diff --git a/tools/run/check_one_click_cocktail_result.sh b/tools/run/check_one_click_cocktail_result.sh +index 030d95c..51bc4a9 100755 +--- a/tools/run/check_one_click_cocktail_result.sh ++++ b/tools/run/check_one_click_cocktail_result.sh +@@ -52,8 +52,10 @@ 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' \ ++ 'PRESS_PRE measured press pre-contact joints' \ ++ 'PRESS_CONTACT measured contact joints' \ ++ 'PRESS_Z_OVERDRIVE from measured contact' \ ++ 'return to PRESS_CONTACT after Z overdrive' \ + 'RG2 soft side-grasp' \ + 'post-grasp lift'; do + if grep -q "${needle}" "${INTEGRATED_LOG}"; then +diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py +index 6d17c7f..62085f6 100644 +--- a/tools/run/run_color_recipe_sequence.py ++++ b/tools/run/run_color_recipe_sequence.py +@@ -267,10 +267,10 @@ def main() -> int: + ) + parser.add_argument( + "--press-lock-contact-joints", +- default="6", ++ default="", + help=( +- "measured sequence로 전달할 contact 조인트 잠금 축. 기본 6: " +- "호환 모드에서만 pre 자세 값으로 J6을 유지합니다. 기본 contact-only 경로에서는 사용하지 않습니다." ++ "measured sequence로 전달할 contact 조인트 잠금 축. 기본 빈 값: " ++ "측정된 PRESS_CONTACT joint를 그대로 사용합니다." + ), + ) + parser.add_argument("--press-pre-lift-m", default="0.080") +diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py +index 8753107..05e0dd7 100755 +--- a/tools/run/run_measured_dispenser_recipe_sequence.py ++++ b/tools/run/run_measured_dispenser_recipe_sequence.py +@@ -2,8 +2,8 @@ + """Run an ordered measured-dispenser recipe loop. +  + For each dispenser ID this composes measured rule-based primitives: +- move/release cup at DISP_PRE/DISP_PLACE -> measured PRESS_CONTACT FK +- -> generated Z-only PRESS_PRE -> measured contact -> Z-only pump stroke ++ move/release cup at DISP_PRE/DISP_PLACE -> measured PRESS_PRE/PRESS_CONTACT ++ -> Z-only pump overdrive -> measured PRESS_CONTACT -> measured PRESS_PRE + -> re-grasp/lift cup at DISP_PLACE. +  + All cup/dispenser positions come from measured calibration data. This runner +@@ -389,18 +389,26 @@ def print_dry_run_group_detail(args: argparse.Namespace, dispenser_id: str, pres + cup_pre = load_cup_pre_place_joints_deg(dispenser_id) + cup_place = load_cup_place_joints_deg(dispenser_id) + if cup_pre is not None and cup_place is not None: +- print( +- f"[PLAN] dispenser {dispenser_id}: cup DISP_PRE={format_joints_deg(cup_pre)} " +- f"-> DISP_PLACE={format_joints_deg(cup_place)}" +- ) ++ print(f"[Azas] DISP_PRE={format_joints_deg(cup_pre)}") ++ print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place)}") ++ print(f"[PLAN] dispenser {dispenser_id}: cup DISP_PRE -> DISP_PLACE -> RELEASE") + else: + print(f"[PLAN] dispenser {dispenser_id}: cup placement fallback=front_hold_poses") +  + press_contact = load_press_ready_joints_deg(dispenser_id) ++ press_pre = load_press_pre_joints_deg(dispenser_id) if press_contact is not None else None + z_overdrive_mm = (max(args.press_depth_m, 0.0) + max(args.press_extra_depth_m, 0.0)) * 1000.0 +- if press_contact is not None: ++ if press_contact is not None and press_pre is not None: ++ print(f"[Azas] PRESS_PRE={format_joints_deg(press_pre)}") ++ print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") ++ print( ++ f"[PLAN] dispenser {dispenser_id}: press PRESS_PRE -> PRESS_CONTACT " ++ f"-> PRESS_Z_OVERDRIVE {z_overdrive_mm:.1f}mm -> PRESS_CONTACT -> PRESS_PRE x{press_count}" ++ ) ++ elif press_contact is not None: ++ print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") + print( +- f"[PLAN] dispenser {dispenser_id}: press CONTACT={format_joints_deg(press_contact)} " ++ f"[PLAN] dispenser {dispenser_id}: press contact-only fallback CONTACT " + f"-> generated PRE=CONTACT+Z{max(args.press_contact_entry_lift_m, 0.0) * 1000.0:.1f}mm " + f"-> PRESS=CONTACT-Z{z_overdrive_mm:.1f}mm x{press_count}" + ) +@@ -1297,11 +1305,48 @@ class IntegratedRecipeMotion: + time.sleep(settle_sec) +  + def move_and_release(self, dispenser_id: str) -> None: ++ cup_pre_joints = load_cup_pre_place_joints_deg(dispenser_id) + cup_place_joints = load_cup_place_joints_deg(dispenser_id) +- if cup_place_joints is not None: ++ if cup_pre_joints is not None and cup_place_joints is not None: ++ print(f"[Azas] DISP_PRE={format_joints_deg(cup_pre_joints)}") ++ print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place_joints)}") + print( + f"[Azas] cup placement: dispenser={dispenser_id} using measured " +- "DISP_PLACE with generated high entry; not front_hold_poses" ++ "DISP_PRE -> DISP_PLACE joint teaching; not front_hold_poses" ++ ) ++ cup_pre_joints = self.normalize_joints_near_current( ++ cup_pre_joints, ++ label=f"move to measured DISP{dispenser_id}_PRE cup-place joints", ++ ) ++ self.movej( ++ cup_pre_joints, ++ label=f"DISP_PRE measured DISP{dispenser_id} cup-place approach", ++ velocity=self.args.move_prehold_velocity, ++ acceleration=self.args.move_prehold_acceleration, ++ ) ++ cup_place_joints = self.normalize_joints_near_current( ++ cup_place_joints, ++ label=f"move to measured DISP{dispenser_id}_PLACE cup-place joints", ++ ) ++ self.movej( ++ cup_place_joints, ++ label=f"DISP_PLACE measured DISP{dispenser_id} cup release", ++ velocity=self.args.move_velocity, ++ accelera \ No newline at end of file diff --git a/contact b/contact new file mode 100644 index 0000000..e69de29 diff --git a/cup_classifier_best.pth b/cup_classifier_best.pth new file mode 100644 index 0000000..76be5dd --- /dev/null +++ b/cup_classifier_best.pth @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d4340580806b8d5b81271eedbd2e99edc21c004a1dbf2beb260e6648b34df377 +size 44790923 diff --git a/docs/developer_orientation.md b/docs/developer_orientation.md index bbe700c..730273e 100644 --- a/docs/developer_orientation.md +++ b/docs/developer_orientation.md @@ -146,7 +146,7 @@ STT | `tumbler_floor_place_demo.launch.py` | 단독 sim | 컵 이동만 볼 때 | | `dispenser_press.launch.py` | 부분 구현 | 디스펜서 press 단독 테스트 | | `dispense_lid_sequence.launch.py` | 부분 구현 | 디스펜서 press + lid close fake/dry-run | -| `rg2_trigger.launch.py` | real support | RG2 Trigger wrapper | +| `robot_connection_control.launch.py` RG2 Node | real support | `azas_gripper/rg2_gripper_node` starts `/jarvis/rg2/*` compatibility services from inside `~/Azas` | | `tumbler_dispenser_gazebo.launch.py` | visual preview | Gazebo asset preview, full physics pipeline 아님 | ## launch 정리 판정 @@ -155,8 +155,8 @@ STT | launch | 판정 | 이유 | 권장 조치 | | --- | --- | --- | --- | -| `mvp_bringup.launch.py` | 삭제 후보 | 오래된 MVP 골격입니다. 현재 컵 pose bridge, jarvis floor-place, real gate 흐름을 쓰지 않습니다. | `deprecated_mvp_bringup.launch.py`로 이름 변경 또는 제거 | -| `hardware_free_demo.launch.py` | 통합 후보 | `tumbler_floor_place_demo.launch.py`와 역할이 겹칩니다. 다만 voice/LLM/demo pose까지 묶는 차이가 있습니다. | RViz demo는 jarvis 쪽으로 통일하고, voice demo만 남길지 결정 | +| `mvp_bringup.launch.py` | 삭제 후보 | 오래된 MVP 골격입니다. 현재 컵 pose bridge, Azas RG2/floor-place, real gate 흐름을 쓰지 않습니다. | `deprecated_mvp_bringup.launch.py`로 이름 변경 또는 제거 | +| `hardware_free_demo.launch.py` | 통합 후보 | `tumbler_floor_place_demo.launch.py`와 역할이 겹칩니다. 다만 voice/LLM/demo pose까지 묶는 차이가 있습니다. | RViz demo는 Azas 쪽으로 통일하고, voice demo만 남길지 결정 | | `simulated_cup_grasp_dryrun.launch.py` | 통합 후보 | simulated cup detection + floor-place dry-run입니다. fake/smoke 경로와 목적이 겹칩니다. | `tools/smoke` 또는 `tools/run` 진입점으로 흡수 | | `gpd_grasp_adapter.launch.py` | 실험 보존 | GPD 외부 grasp adapter 경계입니다. 현재 메인 파이프라인은 아니지만 독립 실험 가치가 있습니다. | `experimental_gpd_grasp_adapter.launch.py`로 이름 명확화 | | `tumbler_floor_place_demo.launch.py` | 유지 | 컵 이동 단독 RViz 확인용입니다. | 단독 stage demo로 유지 | diff --git a/docs/lid_gripper_pipeline.md b/docs/lid_gripper_pipeline.md new file mode 100644 index 0000000..b522f77 --- /dev/null +++ b/docs/lid_gripper_pipeline.md @@ -0,0 +1,164 @@ +# Lid Gripper Pipeline + +This path is for detecting the cup lid center marker, showing a live RealSense preview, +and generating supervised lid grip requests. By default it is plan-only and does +not execute Doosan motion. + +## Runtime Contract + +1. `lid_sticker_detector_node` + - subscribes to RealSense color, aligned depth, and camera info, + - runs the trained YOLO model for class `lid`, + - detects a 30 mm ArUco marker inside the lid bbox, + - projects the ArUco center with aligned depth, + - estimates the local lid plane normal from the surrounding depth patch, + - publishes `/azas/lid_detection` as `azas_interfaces/msg/CupDetection` with status `detected:lid ...`. + - opens an OpenCV preview window by default and overlays the ArUco marker, optional lid bbox, depth, and status. + - pressing `p` in the preview publishes `/jarvis/lid_gripper/grip_request`; it is ignored unless the current frame is `detected:lid`. + +2. `cup_detection_pose_bridge_node` + - runs a second instance configured for lid detections, + - accepts only status prefix `detected:lid`, + - transforms the camera-frame pose into `base_link`, + - publishes `/jarvis/lid_gripper/lid_pose`. + +3. `lid_grip_planner_node` + - subscribes to `/jarvis/lid_gripper/lid_pose`, + - subscribes to `/jarvis/lid_gripper/grip_request` for supervised `p` key requests, + - validates `frame_id == base_link`, + - publishes: + - `/jarvis/lid_gripper/approach_pose` + - `/jarvis/lid_gripper/grasp_pose` + - `/jarvis/lid_gripper/lift_pose` + - `/jarvis/lid_gripper/status` + - only sends Doosan `MoveLine` requests when `enable_hardware:=true`, `hardware_confirm:=ENABLE_REAL_ROBOT_MOTION`, and `allow_service_control_without_moveit:=true`. + +## Safety Assumptions + +- The ArUco marker defines the lid center target. Current motion still uses the configured Doosan `rx/ry/rz` for TCP orientation. +- The grip angle comes from the depth-estimated lid plane normal. +- The hand-eye matrix used by this launch is copied into `src/azas_perception/config/T_gripper2camera.npy` + from `/home/ssu/ros2_ws/src/doosan-robot2/dsr_practice/dsr_practice/Calibration_Tutorial/T_gripper2camera.npy`. + The copied matrix hash was verified to match the original: + `c7fa0eb6aefc6afec2ed36b3672afb0515472142b653ba5e36cb57e114984d4c`. +- The original calibration capture used Doosan `set_tcp("2FG_TCP")` before `get_current_posx()`. + The current RViz/MoveIt-compatible default publishes the matrix under `hand_eye_parent_frame:=link_6`, + matching the existing legacy MoveIt nodes. Before real hardware use, confirm whether the live TF tree + exposes the calibrated TCP separately and override `hand_eye_parent_frame` if needed. +- The default launch is planning-only: no MoveIt execution, no Doosan service call, and no RG2 command. +- `p` is a supervised trigger, not automatic motion. With default parameters it only confirms the latest plan. +- RG2 service calls remain disabled unless `enable_gripper_service_calls:=true` and measured gripper width/force parameters are provided. +- Real lid gripping additionally requires the explicit hardware gates above, verified TF, conservative speed/acceleration values, measured RG2 widths, and operator clearance. The direct `MoveLine` path uses the configured `rx/ry/rz`; validate those values before hardware use. +- `surface_offset_m` moves the grasp target along `offset_axis`, which defaults to the detected lid normal (`local_z`). Use `offset_axis:=base_z` when tuning only vertical clearance. +- `tcp_grasp_offset_x_m/y_m/z_m` is a measured base-frame offset from the detected lid point to the actual RG2 TCP grasp point. It is a gripper/TCP tuning value, not a cup coordinate. +- With hardware enabled, the planner fail-closes before gripper motion when `motion/ikin` is unavailable or fails, and verifies each `MoveLine` target with `aux_control/get_current_posx`. +- RG2 `preopen` is sent only after the approach target is verified. RG2 `grasp` is sent only after the grasp target is verified. +- If the Doosan controller accepts a `MoveLine` service request but the TCP does not reach the target within `motion_verify_timeout_sec`, the sequence stops and reports the last Doosan alarm in `/jarvis/lid_gripper/status`. + +## Launch + +```bash +ros2 launch azas_bringup lid_sticker_grip_planning.launch.py +``` + +If the model is not in `/home/ssu/Downloads/best.pt`, the launch file also checks +`src/cocktail_robot_system/models/best.pt`. You can override it explicitly: + +```bash +ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ + model_path:=/absolute/path/to/best.pt +``` + +For the current 30 mm ArUco marker flow, the defaults are: + +```bash +marker_type:=aruco +require_lid_detection:=true +aruco_dictionary:=DICT_4X4_50 +aruco_marker_id:=-1 +aruco_marker_length_m:=0.03 +``` + +Expected live outputs: + +```bash +ros2 topic echo --once /azas/lid_detection +ros2 topic echo --once /jarvis/lid_gripper/lid_pose +ros2 topic echo --once /jarvis/lid_gripper/status +``` + +If `/jarvis/lid_gripper/lid_pose` is missing while `/azas/lid_detection` is present, +check the TF from `base_link` to `camera_color_optical_frame`. + +## RViz / No-Robot Check + +Use this before connecting the real robot. It should prove only the visual +perception, TF conversion, and lid grip candidate topics. + +1. Start the virtual Doosan/MoveIt or any RViz setup that publishes + `base_link -> link_6`. +2. Start the RealSense driver so it publishes the color image, aligned depth, + camera info, and camera TF tree. +3. Start: + +```bash +ros2 launch azas_bringup lid_sticker_grip_planning.launch.py +``` + +4. Confirm the preview shows `detected:lid`. +5. Confirm these topics publish: + +```bash +ros2 topic echo --once /jarvis/lid_gripper/lid_pose +ros2 topic echo --once /jarvis/lid_gripper/approach_pose +ros2 topic echo --once /jarvis/lid_gripper/grasp_pose +ros2 topic echo --once /jarvis/lid_gripper/lift_pose +``` + +6. In RViz, add `PoseStamped` displays for the three pose topics above. + Seeing these poses does not prove collision-free robot motion; it only + confirms the perception-to-plan geometry. + +Hardware-gated supervised request example: + +```bash +ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ + enable_hardware:=true \ + hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \ + allow_service_control_without_moveit:=true \ + service_prefix:=/dsr01 \ + rx:=108.41 \ + ry:=-176.32 \ + rz:=175.98 \ + offset_axis:=base_z \ + surface_offset_m:=0.0 \ + tcp_grasp_offset_x_m:=-0.006 \ + tcp_grasp_offset_y_m:=-0.045 \ + tcp_grasp_offset_z_m:=-0.064 \ + min_grasp_z_m:=0.0 \ + precheck_ikin:=true \ + verify_motion_reached:=true \ + enable_gripper_service_calls:=true \ + gripper_preopen_width_m:=0.050 \ + gripper_grasp_width_m:=0.033 \ + gripper_force_n:=8.0 +``` + +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/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/docs/onboarding/02-role-map-kor.md b/docs/onboarding/02-role-map-kor.md index 214134b..f6152d4 100644 --- a/docs/onboarding/02-role-map-kor.md +++ b/docs/onboarding/02-role-map-kor.md @@ -34,28 +34,27 @@ source /home/ssu/Azas/install/local_setup.bash | `azas_task_manager` | 레시피 결정과 컵 탐지를 태스크 단계로 조합 | `cocktail_dryrun_sequence_node.py`, `pick_and_align_action_server.py` | 구현됨 (no-motion) | | `azas_perception` | 직립 컵 탐지, 깊이 투영, base_link 기준 컵 자세 발행 | `yolo_tumbler_detector_node.py`, `cup_detection_pose_bridge_node.py` | 구현됨 (`detected:upright`만 pose 발행) | | `azas_motion` | 그라스프 계획 계산 | `alignment.py`, `alignment_executor_node.py` | 계획만, 실행 없음 | -| `azas_gripper` | **내부 placeholder** — 실제 RG2 아님 | `rg2_gripper_node.py` | **미연결** (아래 jarvis 사용) | +| `azas_gripper` | **실제 RG2 경계** + dry-run 모드 | `rg2_gripper_node.py` | `/azas/gripper/open_close`, `/jarvis/rg2/open`, `/jarvis/rg2/close`, `/jarvis/rg2/set_width` | | `azas_calibration` | 실측 캘리브레이션 값 로드/저장 경계 | `calibration_loader_node.py`, `calibration.yaml` | 실측 대기 중 | | `azas_bringup` | 런치 파일과 시스템 설정 조합 | `launch/`, `config/` | 구현됨 | -### Azas에 통합된 외부 드라이버/제어 패키지 +### 외부 드라이버/제어 패키지 | 패키지 | 담당 영역 | 서비스/토픽 | 상태 | |--------|----------|------------|------| -| `jarvis` (rg2_trigger_node) | **실제 RG2 그리퍼** Modbus 제어 | `/jarvis/rg2/open`, `/jarvis/rg2/close` | 구현됨, IP 연결 필요 | -| `jarvis` (tumbler_floor_place_node) | 컵을 디스펜서 아래로 이동 | — | 구현됨, 캘리브레이션 필요 | | `dsr_bringup2` | 두산 M0609 MoveIt 드라이버 | `/dsr01/motion/move_line` 등 | 구현됨, 로봇 IP 필요 | ## RG2 실제 연결 경로 ``` -supervised real-runner scripts / jarvis floor-place path +supervised real-runner scripts / Azas panel path ↓ explicit real-motion confirmation + strict gates 통과 시 -/jarvis/rg2/open ← jarvis/rg2_trigger_node ← Modbus 192.168.1.1 +/jarvis/rg2/open /jarvis/rg2/close +/jarvis/rg2/set_width ← azas_gripper/rg2_gripper_node ← Modbus 192.168.1.1 ``` -`azas_gripper/rg2_gripper_node`는 내부 플레이스홀더로, 실제 RG2를 제어하지 않습니다. +`azas_gripper/rg2_gripper_node`는 `use_real_hardware:=true`일 때 실제 RG2를 제어하고, false일 때 dry-run으로 동작합니다. `pick_and_align_action_server`의 `execution_mode=no_motion`은 `enable_gripper_service_calls=true`가 들어와도 실제 RG2 서비스를 호출하지 않고 실패해야 합니다. no-motion 스모크와 readiness/check 명령은 RG2 서비스의 존재나 타입을 볼 수는 있지만, `/jarvis/rg2/open` 또는 `/jarvis/rg2/close`를 호출했다는 뜻이 아니며 실제 그리퍼 동작 증거도 아닙니다. 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/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 52fd83a..003b3e4 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,29 +149,75 @@ 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-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: 850 !important; + letter-spacing: 0.02em; + } + .direct-dispenser-field { + display: flex; + align-items: center; + gap: 7px; + min-height: 34px; + padding: 0 9px; + border: 1px solid #99f6e4; + border-radius: 8px; + background: #f0fdfa; + color: #115e59; + font-size: 12px; + font-weight: 900; + grid-column: 1 / -1; + } + .direct-dispenser-field input { + width: 180px; + flex: 1; + height: 26px; + border: 1px solid #5eead4; + border-radius: 6px; + padding: 0 8px; + font-size: 13px; + font-weight: 700; + color: #0f172a; + background: #fff; } - .quick-row .recipe-preset { - background: #ecfdf5; - border-color: #a7f3d0; - color: #047857; + .danger-light { + color: var(--red) !important; + background: #fff1f2 !important; + border-color: #fecdd3 !important; } - .danger-light { color: var(--red) !important; background: #fff1f2 !important; border-color: #fecdd3 !important; } details.settings { border-top: 1px dashed var(--line); @@ -193,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; @@ -230,17 +280,18 @@ .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; - } - .flow-empty { - width: 100%; - min-height: 84px; display: grid; - place-items: center; + grid-template-columns: repeat(auto-fill, minmax(178px, 1fr)); + gap: 10px; + padding: 14px; + overflow: visible; + background: #ffffff; + } + .flow-empty { + width: 100%; + min-height: 66px; + display: grid; + place-items: center; color: var(--muted); border: 1px dashed var(--line); border-radius: 14px; @@ -250,45 +301,116 @@ .flow-strip.drop-empty .flow-empty { border-color: #93c5fd; background: #eff6ff; - color: var(--blue-dark); - } + color: var(--blue-dark); + } + .reference-panel { + display: none; + margin-top: 14px; + border: 1px solid #bfdbfe; + border-radius: 16px; + background: #eff6ff; + box-shadow: var(--shadow); + overflow: hidden; + } + .reference-panel summary { + cursor: pointer; + padding: 13px 16px; + color: var(--blue-dark); + font-weight: 950; + list-style: none; + } + .reference-panel summary::-webkit-details-marker { display: none; } + .reference-panel summary::before { content: "▸ "; } + .reference-panel[open] summary::before { content: "▾ "; } + .reference-body { + display: grid; + gap: 9px; + padding: 0 16px 16px; + color: #1e3a8a; + font-size: 13px; + line-height: 1.45; + } + .reference-body pre { + max-height: 360px; + overflow: auto; + margin: 0; + padding: 12px; + border: 1px solid #bfdbfe; + border-radius: 12px; + background: #0f172a; + color: #dbeafe; + font-size: 12px; + line-height: 1.45; + white-space: pre-wrap; + } + .camera-panel { + margin-top: 12px; + border: 1px solid #bfdbfe; + border-radius: 12px; + background: #fff; + box-shadow: var(--shadow); + overflow: hidden; + grid-column: 1 / -1; + grid-row: auto; + max-height: none; + } + .camera-head { + display: flex; + justify-content: space-between; + gap: 12px; + align-items: center; + padding: 14px 16px; + border-bottom: 1px solid var(--line); + background: #f8fafc; + } + .camera-head h2 { margin: 0; font-size: 18px; letter-spacing: -0.01em; } + .camera-help { margin-top: 3px; color: var(--muted); font-size: 13px; line-height: 1.4; } + .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: 420px; + display: grid; + place-items: center; + background: #020617; + } + .camera-frame img { + width: 100%; + max-height: 620px; + object-fit: contain; + display: block; + color: #dbeafe; + font-size: 13px; + text-align: center; + } + .camera-result { + max-height: 260px; + 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; @@ -300,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; @@ -318,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; @@ -388,17 +577,93 @@ color: #075985; font-size: 13px; } - details.command { margin-top: 10px; } - details.command summary { - cursor: pointer; - color: var(--blue-dark); - font-size: 13px; - font-weight: 900; - list-style: none; + .step-run-now { + min-height: 32px; + padding: 0 10px; + border-radius: 9px; + background: #fee2e2; + border: 1px solid #fecaca; + color: #991b1b; + font-size: 13px; + font-weight: 850; + } + .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); + } + .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); + background: #fff; + box-shadow: 0 24px 60px rgba(15, 23, 42, 0.28); + overflow: hidden; + } + .command-modal-head, + .command-tools { + display: flex; + 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; + 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; } - details.command summary::-webkit-details-marker { display: none; } - details.command summary::before { content: "▸ "; } - details.command[open] summary::before { content: "▾ "; } .step code { display: block; margin-top: 7px; @@ -410,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); } @@ -438,17 +703,19 @@ } .log-panel { - position: sticky; - top: 178px; - height: calc(100vh - 196px); - min-height: 520px; + position: static; + height: min(78vh, 860px); + min-height: 620px; 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; @@ -469,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; } } @@ -494,18 +761,64 @@ .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; } + #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; } + .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는 현장 실행 후보에서 숨김
단계 로드 전 @@ -521,24 +834,31 @@

Azas Robot Pipeline Control

- + + -
- - - - - - - - - - - - +
+
현재 후보 순서: tmux 연결 준비 → 수빈 STT/주문 UI → 색상 스캔 → 창현 side-grip / 소명 누운컵(현재 점검 필요) → 통합 디스펜서 → 강모 ArUco 뚜껑 체결 → 쉐이킹.
+ +
+ + + + + + + + + + + + +
@@ -562,21 +882,150 @@

Azas Robot Pipeline Control

- + +
- + +
+ 강개발자 뚜껑 잡고 닫기 참고 명령어 +
+
+ 뚜껑 잡고 닫기 버튼은 강개발자 로직인 lid_grip_close 단계입니다. + 이 로직은 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이지만, 실제 인쇄 스티커는 반드시 실측값으로 맞춰야 합니다. + 디스펜서 프레스나 컵홀더 배치가 아니라 뚜껑 파지→이동→닫기 전용 실제모션입니다. + 실행 전 로봇 연결, 연결 확인, 그리퍼 연결, 카메라 연결이 필요합니다. +
+ 
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_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 \
+  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.yaml의 cup_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 \
+  --approach-velocity 80.0 \
+  --place-velocity 80.0 \
+  --retreat-velocity 80.0 \
+  --place-final-y-offset-m -0.010 \
+  --place-final-z-offset-m -0.020 \
+  --execute --confirm ENABLE_CUP_HOLDER_PLACE
+
+
+ +
+ 소명 누운 컵 세우기 참고 명령어 +
+
+ 소명 누운 컵 세우기 버튼은 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
+source /home/ssu/Azas/install/setup.bash
+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
+
+
+

선택 실행 순서

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

선택 실행 순서

-

파이프라인 단계

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

디버그 단계

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

파이프라인 단계

실행 로그
+
@@ -605,16 +1055,76 @@

파이프라인 단계

서버 연결 대기...
+ + + + + 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/local_models/README.md b/local_models/README.md new file mode 100644 index 0000000..03db2a9 --- /dev/null +++ b/local_models/README.md @@ -0,0 +1,13 @@ +# Local Model Files + +Place or symlink runtime model weights here on each robot PC. + +Expected file: + +```text +local_models/best.pt +``` + +The `.pt` file is intentionally not committed because model weights are large +local runtime assets. Use `tools/setup/link_yolo_model.sh` to link an existing +downloaded model into this stable repo-local path. diff --git a/models/mediapipe/hand_landmarker.task b/models/mediapipe/hand_landmarker.task new file mode 100644 index 0000000..0d53faf Binary files /dev/null and b/models/mediapipe/hand_landmarker.task differ diff --git a/omx_wiki/index.md b/omx_wiki/index.md index f0f302b..345ce9a 100644 --- a/omx_wiki/index.md +++ b/omx_wiki/index.md @@ -1,8 +1,14 @@ # Wiki Index -> 2 pages | Last updated: 2026-05-19T09:32:54.476Z +> 8 pages | Last updated: 2026-06-11T23:23:12.255Z ## 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 +- [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 73cc149..cde1ac1 100644 --- a/omx_wiki/log.md +++ b/omx_wiki/log.md @@ -1,5 +1,21 @@ # 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. + +## [2026-06-10T16:48:12+09:00] update +- **Pages:** robot-operation-rules.md +- **Summary:** Recorded operator confirmation that D1-D4 dispenser press contacts are saved and confirmed; D4 press-check failure was cable disconnect, not calibration. + +## [2026-06-10T15:50:45+09:00] add +- **Pages:** robot-operation-rules.md +- **Summary:** Recorded the operator directive that dry-run / plan-only / no-execute commands are banned as substitutes for real robot motion checks. + ## [2026-05-18T10:06:59.396Z] ingest - **Pages:** azas-real-robot-handoff-2026-05-18-dispenser-shake-panel.md - **Summary:** Created new page "Azas Real Robot Handoff 2026-05-18 Dispenser Shake Panel" @@ -50,3 +66,25 @@ - **Pages:** session-log-2026-05-19-0-4as20g.md - **Summary:** Auto-captured session log for omx-1779176308980-4as20g +## [2026-05-28T02:01:49.697Z] session-end +- **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 + +## [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 +## [2026-06-10T10:52:52.607Z] session-end +- **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/new_dispenser_coordinate.md b/omx_wiki/new_dispenser_coordinate.md new file mode 100644 index 0000000..e69de29 diff --git a/omx_wiki/robot-operation-rules.md b/omx_wiki/robot-operation-rules.md new file mode 100644 index 0000000..3cdf863 --- /dev/null +++ b/omx_wiki/robot-operation-rules.md @@ -0,0 +1,65 @@ +# 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: 드라이런은 앞으로 금지. + +For dispenser, press, cup placement, cup re-grasp, and other real robot motion checks, do not suggest or run dry-run / plan-only / no-execute commands as a substitute for motion validation. + +Required behavior: + +- When the user asks to see robot movement, use real execution commands only, with explicit `--execute` and the required `--confirm` phrase. +- Prefer low-speed, bounded, single-stage checks first, especially for dispenser press/contact verification. +- If actual execution is unsafe or blocked, state the concrete blocker and stop. Do not replace the requested real-motion check with a dry-run. +- RViz preview is only a visual aid when explicitly requested. It does not replace real-motion validation. +- Do not invent cup, dispenser, press, or collision coordinates. Use measured calibration values, robot services, or the vision pipeline. + +Stop condition: + +- Stop before motion if the command would use missing/ambiguous calibration, unknown TCP state, inconsistent FK, unavailable robot services, or an identified collision risk. + +## 2026-06-10 Dispenser Press Contacts Confirmed + +Operator note at 2026-06-10T16:48:12+09:00: all dispenser `PRESS_CONTACT` values have been saved. + +Confirmed state: + +- D1 press-only real-motion check completed and `press_contact_status: measured_confirmed`. +- D2 press-only real-motion check completed and `press_contact_status: measured_confirmed`. +- D3 press-only real-motion check completed and `press_contact_status: measured_confirmed`. +- D4 press-only real-motion check also works normally. The earlier observed `MoveLine returned success=false` was caused by a cable disconnect, not by the saved D4 contact. D4 status is `press_contact_status: measured_confirmed`. + +Do not resurrect the stale `PRESS1_CONTACT invalid_reteach_required` note unless the operator newly invalidates the saved D1 contact. + +## 2026-06-10 Dispenser Cup Place Mapping + +Operator-confirmed correction: + +- Previous DISP2 cup place/re-grasp pose was actually the physical DISP4 cup position. +- Previous DISP3 cup place/re-grasp pose was actually the physical DISP2 cup position. +- Physical DISP3 cup place/re-grasp pose still needs re-teaching. + +Current intended calibration state: + +- D1 cup place: measured confirmed. +- D2 cup place: uses the previous D3 measured cup place values. +- D3 cup place: re-taught and measured confirmed at 2026-06-10T18:41-18:42+09:00. +- D4 cup place: uses the previous D2 measured cup place values. + +Re-grasp path correction: + +- Do not lower to measured `DISP_PRE` before the cup re-grasp intermediate point. +- After press, use `HOME joint -> high rear-entry -> lowered rear-entry -> final cup grasp`. +- For cup placement/re-grasp offset, `--move-release-offset-x-m -0.030` is 10 mm closer to the dispenser than `-0.040`. diff --git a/omx_wiki/session-log-2026-05-28-3-adlm15.md b/omx_wiki/session-log-2026-05-28-3-adlm15.md new file mode 100644 index 0000000..0fe2bcf --- /dev/null +++ b/omx_wiki/session-log-2026-05-28-3-adlm15.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-05-28" +tags: ["session-log", "auto-captured"] +created: 2026-05-28T02:01:49.697Z +updated: 2026-05-28T02:01:49.697Z +sources: ["omx-1779933651513-adlm15"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-05-28 + +Auto-captured session metadata. +Session ID: omx-1779933651513-adlm15 + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. 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/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/omx_wiki/session-log-2026-06-10-7-prx6mu.md b/omx_wiki/session-log-2026-06-10-7-prx6mu.md new file mode 100644 index 0000000..bd6e116 --- /dev/null +++ b/omx_wiki/session-log-2026-06-10-7-prx6mu.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-06-10" +tags: ["session-log", "auto-captured"] +created: 2026-06-10T10:52:52.607Z +updated: 2026-06-10T10:52:52.607Z +sources: ["omx-1780826697197-prx6mu"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-06-10 + +Auto-captured session metadata. +Session ID: omx-1780826697197-prx6mu + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. 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/pressed_z b/pressed_z new file mode 100644 index 0000000..e69de29 diff --git a/src/azas_bringup/azas_bringup/collision_scene_rviz_publisher.py b/src/azas_bringup/azas_bringup/collision_scene_rviz_publisher.py new file mode 100644 index 0000000..097a899 --- /dev/null +++ b/src/azas_bringup/azas_bringup/collision_scene_rviz_publisher.py @@ -0,0 +1,380 @@ +#!/usr/bin/env python3 +"""Publish a high-visibility collision scene for RViz review.""" + +from __future__ import annotations + +from pathlib import Path + +import rclpy +import yaml +from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import Point, Pose +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from visualization_msgs.msg import Marker, MarkerArray + + +WALL_THICKNESS = 0.04 + + +def transient_qos(depth: int = 10) -> QoSProfile: + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + +def _read_yaml(path: Path) -> dict: + data = yaml.safe_load(path.read_text()) + return data if isinstance(data, dict) else {} + + +def _box_marker( + marker_id: int, + namespace: str, + frame_id: str, + stamp, + center: tuple[float, float, float], + size: tuple[float, float, float], + color: tuple[float, float, float, float], + label: str = "", +) -> list[Marker]: + marker = Marker() + marker.header.frame_id = frame_id + marker.header.stamp = stamp + marker.ns = namespace + marker.id = marker_id + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose = Pose() + marker.pose.position.x, marker.pose.position.y, marker.pose.position.z = center + marker.pose.orientation.w = 1.0 + marker.scale.x, marker.scale.y, marker.scale.z = size + marker.color.r, marker.color.g, marker.color.b, marker.color.a = color + markers = [marker] + + if label: + text = Marker() + text.header.frame_id = frame_id + text.header.stamp = stamp + text.ns = f"{namespace}_labels" + text.id = marker_id + 10000 + text.type = Marker.TEXT_VIEW_FACING + text.action = Marker.ADD + text.pose = Pose() + text.pose.position.x = center[0] + text.pose.position.y = center[1] + text.pose.position.z = center[2] + size[2] / 2.0 + 0.05 + text.pose.orientation.w = 1.0 + text.scale.z = 0.045 + text.color.r = 1.0 + text.color.g = 1.0 + text.color.b = 1.0 + text.color.a = 1.0 + text.text = label + markers.append(text) + return markers + + +def _box_edge_marker( + marker_id: int, + namespace: str, + frame_id: str, + stamp, + center: tuple[float, float, float], + size: tuple[float, float, float], + color: tuple[float, float, float, float], + line_width: float = 0.018, +) -> Marker: + cx, cy, cz = center + sx, sy, sz = (value / 2.0 for value in size) + corners = [ + (cx - sx, cy - sy, cz - sz), + (cx + sx, cy - sy, cz - sz), + (cx + sx, cy + sy, cz - sz), + (cx - sx, cy + sy, cz - sz), + (cx - sx, cy - sy, cz + sz), + (cx + sx, cy - sy, cz + sz), + (cx + sx, cy + sy, cz + sz), + (cx - sx, cy + sy, cz + sz), + ] + edge_indices = [ + (0, 1), + (1, 2), + (2, 3), + (3, 0), + (4, 5), + (5, 6), + (6, 7), + (7, 4), + (0, 4), + (1, 5), + (2, 6), + (3, 7), + ] + + marker = Marker() + marker.header.frame_id = frame_id + marker.header.stamp = stamp + marker.ns = namespace + marker.id = marker_id + marker.type = Marker.LINE_LIST + marker.action = Marker.ADD + marker.pose.orientation.w = 1.0 + marker.scale.x = line_width + marker.color.r, marker.color.g, marker.color.b, marker.color.a = color + for start, end in edge_indices: + for corner_index in (start, end): + point = Point() + point.x, point.y, point.z = corners[corner_index] + marker.points.append(point) + return marker + + +def _box_with_edges( + marker_id: int, + namespace: str, + frame_id: str, + stamp, + center: tuple[float, float, float], + size: tuple[float, float, float], + fill_color: tuple[float, float, float, float], + edge_color: tuple[float, float, float, float], + label: str = "", +) -> list[Marker]: + markers = _box_marker(marker_id, namespace, frame_id, stamp, center, size, fill_color, label) + markers.append( + _box_edge_marker( + marker_id + 20000, + f"{namespace}_edges", + frame_id, + stamp, + center, + size, + edge_color, + ) + ) + return markers + + +class CollisionSceneRvizPublisher(Node): + def __init__(self) -> None: + super().__init__("collision_scene_rviz_publisher") + self.declare_parameter("frame_id", "base_link") + self.declare_parameter( + "safety_config_path", + str(Path(get_package_share_directory("azas_bringup")) / "config" / "safety.yaml"), + ) + self.declare_parameter( + "dispenser_collision_config_path", + str( + Path(get_package_share_directory("azas_bringup")) + / "config" + / "measured_dispenser_collision.yaml" + ), + ) + self.declare_parameter( + "calibration_path", + str(Path(get_package_share_directory("azas_bringup")) / "config" / "calibration.yaml"), + ) + self.declare_parameter("publish_workspace_ceiling", False) + + self.frame_id = self.get_parameter("frame_id").get_parameter_value().string_value + self.publish_workspace_ceiling = ( + self.get_parameter("publish_workspace_ceiling").get_parameter_value().bool_value + ) + self.safety_path = Path( + self.get_parameter("safety_config_path").get_parameter_value().string_value + ) + self.dispenser_path = Path( + self.get_parameter("dispenser_collision_config_path").get_parameter_value().string_value + ) + self.calibration_path = Path( + self.get_parameter("calibration_path").get_parameter_value().string_value + ) + self.publisher = self.create_publisher( + MarkerArray, "/azas/collision_scene/markers", transient_qos(10) + ) + self.default_marker_publisher = self.create_publisher( + MarkerArray, "/visualization_marker_array", transient_qos(10) + ) + self.timer = self.create_timer(1.0, self._publish) + self._publish() + self.get_logger().info( + "Publishing high-visibility collision scene to " + "/azas/collision_scene/markers and /visualization_marker_array" + ) + + def _workspace_markers(self, stamp, start_id: int) -> tuple[list[Marker], int]: + safety = _read_yaml(self.safety_path) + bounds = safety.get("motion", {}).get("workspace_bounds_m", {}) + x_min = float(bounds.get("x_min", -0.25)) + x_max = float(bounds.get("x_max", 1.15)) + y_min = float(bounds.get("y_min", -0.60)) + y_max = float(bounds.get("y_max", 0.60)) + z_min = float(bounds.get("z_min", 0.07)) + z_max = float(bounds.get("z_max", 0.80)) + + height = z_max - z_min + cx = (x_min + x_max) / 2.0 + cy = (y_min + y_max) / 2.0 + cz = z_min + height / 2.0 + dx = x_max - x_min + dy = y_max - y_min + t = WALL_THICKNESS + walls = [ + ("+Y safety", (cx, y_max + t / 2.0, cz), (dx + 2 * t, t, height)), + ("-Y safety", (cx, y_min - t / 2.0, cz), (dx + 2 * t, t, height)), + ("+X safety", (x_max + t / 2.0, cy, cz), (t, dy, height)), + ("-X safety", (x_min - t / 2.0, cy, cz), (t, dy, height)), + ("floor", (cx, cy, z_min - t / 2.0), (dx, dy, t)), + ] + if self.publish_workspace_ceiling: + walls.append(("ceiling", (cx, cy, z_max + t / 2.0), (dx, dy, t))) + + markers: list[Marker] = [] + marker_id = start_id + for label, center, size in walls: + markers.extend( + _box_with_edges( + marker_id, + "safety_workspace_green", + self.frame_id, + stamp, + center, + size, + (0.0, 0.95, 0.25, 0.18), + (0.0, 1.0, 0.15, 1.0), + label, + ) + ) + marker_id += 1 + return markers, marker_id + + def _table_markers(self, stamp, start_id: int) -> tuple[list[Marker], int]: + calibration = _read_yaml(self.calibration_path) + table = calibration.get("table", {}) + if not table: + return [], start_id + center_xy = table.get("center_xy_m", [0.45, 0.0]) + size_xy = table.get("size_xy_m", [1.2, 1.0]) + thickness = float(table.get("thickness_m", 0.04)) + surface_z = float(table.get("surface_z_m", 0.0)) + markers = _box_with_edges( + start_id, + "table_collision", + self.frame_id, + stamp, + (float(center_xy[0]), float(center_xy[1]), surface_z - thickness / 2.0), + (float(size_xy[0]), float(size_xy[1]), thickness), + (0.48, 0.32, 0.16, 0.42), + (0.85, 0.55, 0.20, 1.0), + "table", + ) + return markers, start_id + 1 + + def _dispenser_markers(self, stamp, start_id: int) -> tuple[list[Marker], int]: + dispenser = _read_yaml(self.dispenser_path) + markers: list[Marker] = [] + marker_id = start_id + for object_id, obj in (dispenser.get("estimated_collision_objects") or {}).items(): + if obj.get("type") != "box": + continue + center = tuple(float(value) for value in obj["center_xyz_m"]) + size = tuple(float(value) for value in obj["size_xyz_m"]) + markers.extend( + _box_with_edges( + marker_id, + "dispenser_collision_orange", + self.frame_id, + stamp, + center, + size, + (1.0, 0.33, 0.0, 0.42), + (1.0, 0.55, 0.0, 1.0), + object_id, + ) + ) + marker_id += 1 + + for hold_name, hold in (dispenser.get("front_hold_poses") or {}).items(): + xyz = hold.get("position_xyz_m") + if not xyz: + continue + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = stamp + marker.ns = "dispenser_front_hold_green" + marker.id = marker_id + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose = Pose() + marker.pose.position.x = float(xyz[0]) + marker.pose.position.y = float(xyz[1]) + marker.pose.position.z = float(xyz[2]) + marker.pose.orientation.w = 1.0 + marker.scale.x = marker.scale.y = marker.scale.z = 0.035 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.25 + marker.color.a = 1.0 + markers.append(marker) + marker_id += 1 + + label = Marker() + label.header.frame_id = self.frame_id + label.header.stamp = stamp + label.ns = "dispenser_front_hold_labels" + label.id = marker_id + label.type = Marker.TEXT_VIEW_FACING + label.action = Marker.ADD + label.pose = Pose() + label.pose.position.x = float(xyz[0]) + label.pose.position.y = float(xyz[1]) + label.pose.position.z = float(xyz[2]) + 0.06 + label.pose.orientation.w = 1.0 + label.scale.z = 0.04 + label.color.r = 0.0 + label.color.g = 1.0 + label.color.b = 0.25 + label.color.a = 1.0 + label.text = hold_name + markers.append(label) + marker_id += 1 + return markers, marker_id + + def _publish(self) -> None: + stamp = self.get_clock().now().to_msg() + clear = Marker() + clear.header.frame_id = self.frame_id + clear.header.stamp = stamp + clear.action = Marker.DELETEALL + + markers: list[Marker] = [clear] + marker_id = 0 + for builder in (self._workspace_markers, self._table_markers, self._dispenser_markers): + built, marker_id = builder(stamp, marker_id) + markers.extend(built) + marker_array = MarkerArray(markers=markers) + self.publisher.publish(marker_array) + self.default_marker_publisher.publish(marker_array) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = CollisionSceneRvizPublisher() + try: + 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_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/azas_bringup/rule_motion_joint_preview_node.py b/src/azas_bringup/azas_bringup/rule_motion_joint_preview_node.py new file mode 100644 index 0000000..3df1dfa --- /dev/null +++ b/src/azas_bringup/azas_bringup/rule_motion_joint_preview_node.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +"""RViz-only joint-state preview driven by the rule-motion Path.""" + +from __future__ import annotations + +import math + +import rclpy +from nav_msgs.msg import Path +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from sensor_msgs.msg import JointState + + +class RuleMotionJointPreviewNode(Node): + def __init__(self) -> None: + super().__init__("rule_motion_joint_preview_node") + self.declare_parameter("path_topic", "/azas/dispenser_sequence/plan") + self.declare_parameter("joint_state_topic", "/joint_states") + self.declare_parameter("publish_rate_hz", 30.0) + self.declare_parameter("frames_per_pose", 12) + self.declare_parameter("loop_preview", True) + + self.path: Path | None = None + self.segment_index = 0 + self.frame_index = 0 + self.publisher = self.create_publisher( + JointState, + str(self.get_parameter("joint_state_topic").value), + 10, + ) + self.create_subscription( + Path, + str(self.get_parameter("path_topic").value), + self._on_path, + 10, + ) + period = 1.0 / max(float(self.get_parameter("publish_rate_hz").value), 1.0) + self.create_timer(period, self._publish) + self.get_logger().info( + "RViz-only rule motion joint preview publishing /joint_states from dispenser Path" + ) + + def _on_path(self, msg: Path) -> None: + if len(msg.poses) < 2: + return + if self.path is None or len(self.path.poses) != len(msg.poses): + self.segment_index = 0 + self.frame_index = 0 + self.get_logger().info(f"Received rule motion Path with {len(msg.poses)} poses") + self.path = msg + + def _publish(self) -> None: + if self.path is None or len(self.path.poses) < 2: + return + frames_per_pose = max(int(self.get_parameter("frames_per_pose").value), 1) + pose_count = len(self.path.poses) + start = self.path.poses[self.segment_index].pose.position + end = self.path.poses[min(self.segment_index + 1, pose_count - 1)].pose.position + ratio = self.frame_index / frames_per_pose + ratio = 0.5 - 0.5 * math.cos(math.pi * ratio) + x = start.x + (end.x - start.x) * ratio + y = start.y + (end.y - start.y) * ratio + z = start.z + (end.z - start.z) * ratio + self._publish_joint_state(x, y, z) + + self.frame_index += 1 + if self.frame_index > frames_per_pose: + self.frame_index = 0 + self.segment_index += 1 + if self.segment_index >= pose_count - 1: + if bool(self.get_parameter("loop_preview").value): + self.segment_index = 0 + else: + self.segment_index = pose_count - 2 + + def _publish_joint_state(self, x: float, y: float, z: float) -> None: + radius = max(math.hypot(x, y), 0.1) + base = math.atan2(y, x) + shoulder = -0.65 + (0.42 - min(max(z, 0.08), 0.70)) * 1.15 + elbow = 1.35 - min(max(radius - 0.20, 0.0), 0.70) * 1.30 + wrist1 = -0.65 + min(max(z - 0.18, 0.0), 0.50) * 1.25 + wrist2 = 1.10 + 0.25 * math.sin(self.segment_index * 0.7) + wrist3 = -base * 0.45 + 0.2 * math.sin(self.frame_index * 0.25) + + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + msg.position = [base, shoulder, elbow, wrist1, wrist2, wrist3] + self.publisher.publish(msg) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = RuleMotionJointPreviewNode() + try: + 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_bringup/config/calibration.yaml b/src/azas_bringup/config/calibration.yaml index 883426a..198e065 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link cup_offsets: default: tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 @@ -25,34 +38,102 @@ outlet: pose_rpy_rad: null # 확인 필요 clearance_m: null # 확인 필요: rim/outlet 안전 간격 +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + # Fixed dispenser-number to measured-pose mapping. # STT/LLM may select only these IDs. Coordinates must be filled only from # measured teaching/calibration data in base_link; never from STT, LLM, or VLA. dispenser_outlets: "1": - outlet_pose_xyz_m: null # 확인 필요: base_link 기준 토출구 중심 - outlet_pose_rpy_rad: null - press_pose_xyz_m: null # 확인 필요: base_link 기준 누름/작동 위치 - press_pose_rpy_rad: null + cup_pre_place_joints_deg: [12.87, 45.19, 120.01, 195.36, 75.59, -93.10] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.25, 46.48, 101.91, 13.06, -55.73, 84.67] # operator measured, 2026-06-11 + 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] + cup_place_status: measured_confirmed # operator measured, 2026-06-11 + press_pose_xyz_m: [0.909159, 0.075173, 0.420149] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [0.057995, 1.618397, 0.535828] + press_pose_rpy_deg: [3.323, 92.727, 30.701] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [5.22, 29.22, 69.35, 14.20, -6.03, -13.14] # operator measured, 2026-06-11 + press_contact_status: measured_confirmed # operator measured, 2026-06-11 clearance_m: null + press_common_pre_joints_deg: [-2.87, -12.60, 71.14, 8.30, 55.57, -4.25] # operator measured, 2026-06-11 "2": - outlet_pose_xyz_m: null - outlet_pose_rpy_rad: null - press_pose_xyz_m: null - press_pose_rpy_rad: null + cup_pre_place_joints_deg: [5.70, 43.85, 122.71, 189.65, 76.38, -93.10] # operator measured, 2026-06-10 + cup_place_joints_deg: [3.43, 46.02, 103.63, 5.85, -57.67, 84.67] # operator measured, 2026-06-11 + 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] + cup_place_status: measured_confirmed # operator measured, 2026-06-11 + press_pose_xyz_m: [0.899597, 0.022643, 0.423377] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.004332, 1.635329, 0.536580] + press_pose_rpy_deg: [0.248, 93.697, 30.744] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.82, 27.25, 70.94, 14.20, -4.63, -13.14] # operator measured, 2026-06-11 clearance_m: null + press_contact_status: measured_confirmed # operator measured, 2026-06-11 "3": + cup_pre_place_joints_deg: [-4.55, 42.44, 117.82, 177.61, 67.77, -89.19] # operator measured, 2026-06-10 + cup_place_joints_deg: [-0.76, 47.06, 102.71, 4.00, -57.65, 84.67] # operator measured, 2026-06-11 outlet_pose_xyz_m: null + outlet_pose_quaternion_xyzw: null outlet_pose_rpy_rad: null - press_pose_xyz_m: null - press_pose_rpy_rad: null + outlet_pose_rpy_deg: null + cup_place_status: measured_confirmed # operator measured, 2026-06-11 + press_pose_xyz_m: [0.903240, -0.033608, 0.422720] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [3.083555, -1.633446, -2.605057] + press_pose_rpy_deg: [176.675, -93.590, -149.259] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-1.76, 27.85, 70.23, 14.20, -4.63, -13.14] # operator measured, 2026-06-11 clearance_m: null + press_contact_status: measured_confirmed # operator measured, 2026-06-11 "4": - outlet_pose_xyz_m: null - outlet_pose_rpy_rad: null - press_pose_xyz_m: null - press_pose_rpy_rad: null + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # mapped from previous DISP2; operator confirmed physical DISP4, 2026-06-10 + cup_place_joints_deg: [-7.60, 47.24, 101.73, -5.63, -57.63, 92.82] # operator measured, 2026-06-11 + 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] + cup_place_status: measured_confirmed # operator measured, 2026-06-11 + press_pose_xyz_m: [0.894757, -0.090838, 0.424022] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [3.019592, -1.634845, -2.604996] + press_pose_rpy_deg: [173.010, -93.670, -149.255] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-5.42, 27.16, 71.00, 14.20, -4.63, -13.14] # operator measured, 2026-06-11 clearance_m: null + press_contact_status: measured_confirmed # operator measured, 2026-06-11 cup_holder: parent_frame: base_link diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-111504 b/src/azas_bringup/config/calibration.yaml.bak-20260610-111504 new file mode 100644 index 0000000..0407347 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-111504 @@ -0,0 +1,157 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-7.38, 27.46, 75.42, 146.62, 8.12, -141.99] # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.79, 27.06, 72.09, 167.98, -0.28, -161.56] # operator measured, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-111645 b/src/azas_bringup/config/calibration.yaml.bak-20260610-111645 new file mode 100644 index 0000000..08735cd --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-111645 @@ -0,0 +1,157 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.680509, 0.110427, 0.439154] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.028002, -1.839618, -0.034249] + press_pose_rpy_deg: [173.492, -105.402, -1.962] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [12.13, 30.96, 61.30, -52.54, 22.26, 233.64] # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.79, 27.06, 72.09, 167.98, -0.28, -161.56] # operator measured, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-111836 b/src/azas_bringup/config/calibration.yaml.bak-20260610-111836 new file mode 100644 index 0000000..597ac21 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-111836 @@ -0,0 +1,157 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.680509, 0.110427, 0.439154] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.028002, -1.839618, -0.034249] + press_pose_rpy_deg: [173.492, -105.402, -1.962] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [12.13, 30.96, 61.30, -52.54, 22.26, 233.64] # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.665204, 0.024690, 0.421117] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.016312, 1.742614, -3.076090] + press_pose_rpy_deg: [0.935, 99.844, -176.247] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [2.17, 26.67, 72.53, -49.56, 1.01, 233.63] # operator measured, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-112001 b/src/azas_bringup/config/calibration.yaml.bak-20260610-112001 new file mode 100644 index 0000000..f39ac26 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-112001 @@ -0,0 +1,157 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.680509, 0.110427, 0.439154] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.028002, -1.839618, -0.034249] + press_pose_rpy_deg: [173.492, -105.402, -1.962] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [12.13, 30.96, 61.30, -52.54, 22.26, 233.64] # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.665204, 0.024690, 0.421117] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.016312, 1.742614, -3.076090] + press_pose_rpy_deg: [0.935, 99.844, -176.247] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [2.17, 26.67, 72.53, -49.56, 1.01, 233.63] # operator measured, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-2.78, 27.45, 72.98, -52.58, -4.56, 233.63] # operator measured, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-162844 b/src/azas_bringup/config/calibration.yaml.bak-20260610-162844 new file mode 100644 index 0000000..7a3358f --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-162844 @@ -0,0 +1,158 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.680509, 0.110427, 0.439154] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.028002, -1.839618, -0.034249] + press_pose_rpy_deg: [173.492, -105.402, -1.962] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-7.38, 27.46, 75.42, 146.62, 8.12, -141.99] # operator measured PRESS1, 2026-06-10 + press_contact_status: invalid_reteach_required # operator reported wrong PRESS1 contact, 2026-06-10; block real press until re-taught + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.665204, 0.024690, 0.421117] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.016312, 1.742614, -3.076090] + press_pose_rpy_deg: [0.935, 99.844, -176.247] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.79, 27.06, 72.09, 167.98, -0.28, -161.56] # operator measured PRESS2, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured PRESS3, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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.660295, -0.083131, 0.405622] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [0.008298, 1.664692, -3.119918] + press_pose_rpy_deg: [0.475, 95.380, -178.758] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured PRESS4, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-163024 b/src/azas_bringup/config/calibration.yaml.bak-20260610-163024 new file mode 100644 index 0000000..3549f40 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-163024 @@ -0,0 +1,158 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.896194, 0.078386, 0.388159] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.051897, -1.843650, -2.758933] + press_pose_rpy_deg: [174.861, -105.633, -158.075] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [10.72, 29.84, 61.31, -45.42, 21.02, 39.84] # operator measured, 2026-06-10 + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.665204, 0.024690, 0.421117] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.016312, 1.742614, -3.076090] + press_pose_rpy_deg: [0.935, 99.844, -176.247] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.79, 27.06, 72.09, 167.98, -0.28, -161.56] # operator measured PRESS2, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured PRESS3, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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.660295, -0.083131, 0.405622] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [0.008298, 1.664692, -3.119918] + press_pose_rpy_deg: [0.475, 95.380, -178.758] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured PRESS4, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-163302 b/src/azas_bringup/config/calibration.yaml.bak-20260610-163302 new file mode 100644 index 0000000..f3ee21c --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-163302 @@ -0,0 +1,158 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.897411, 0.077654, 0.384322] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.041566, -1.889330, -2.756159] + press_pose_rpy_deg: [174.269, -108.251, -157.916] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [10.80, 30.88, 57.88, -38.19, 25.14, 32.93] # operator measured, 2026-06-10 + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.665204, 0.024690, 0.421117] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.016312, 1.742614, -3.076090] + press_pose_rpy_deg: [0.935, 99.844, -176.247] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.79, 27.06, 72.09, 167.98, -0.28, -161.56] # operator measured PRESS2, 2026-06-10 + clearance_m: null + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured PRESS3, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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.660295, -0.083131, 0.405622] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [0.008298, 1.664692, -3.119918] + press_pose_rpy_deg: [0.475, 95.380, -178.758] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured PRESS4, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-163313 b/src/azas_bringup/config/calibration.yaml.bak-20260610-163313 new file mode 100644 index 0000000..d3a61c2 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-163313 @@ -0,0 +1,159 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.897411, 0.077654, 0.384322] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.041566, -1.889330, -2.756159] + press_pose_rpy_deg: [174.269, -108.251, -157.916] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [10.80, 30.88, 57.88, -38.19, 25.14, 32.93] # operator measured, 2026-06-10 + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.894031, 0.023056, 0.383724] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.006618, 1.749987, 0.448575] + press_pose_rpy_deg: [0.379, 100.267, 25.701] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.90, 27.14, 71.09, -27.11, 2.29, 23.18] # operator measured, 2026-06-10 + clearance_m: null + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured PRESS3, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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.660295, -0.083131, 0.405622] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [0.008298, 1.664692, -3.119918] + press_pose_rpy_deg: [0.475, 95.380, -178.758] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured PRESS4, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/calibration.yaml.bak-20260610-163621 b/src/azas_bringup/config/calibration.yaml.bak-20260610-163621 new file mode 100644 index 0000000..4b12ac6 --- /dev/null +++ b/src/azas_bringup/config/calibration.yaml.bak-20260610-163621 @@ -0,0 +1,159 @@ +# PDF/wiki-derived MVP-1 calibration placeholders. +# Real robot calibration is represented only by the top-level fields below. +# Null/확인 필요 values are intentional fail-closed markers; replace them only +# with measured robot-cell, hand-eye, MoveIt, or tool/TCP evidence. +frames: + base_frame: base_link + camera_frame: null # 확인 필요: 실제 depth camera frame_id + 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 + 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. +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. +color_scan_pose: + 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] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: null + cartesian_frame: base_link +cup_offsets: + default: + tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 + status: 확인 필요 +outlet: + outlet_id: dispenser_outlet + parent_frame: base_link + pose_xyz_m: null # 확인 필요: teaching/calibration으로 측정 + pose_rpy_rad: null # 확인 필요 + clearance_m: null # 확인 필요: rim/outlet 안전 간격 + +table: + parent_frame: base_link + source: operator_confirmed_base_link_bounds + status: measured_confirmed + surface_z_m: 0.000 + thickness_m: 0.040 + center_xy_m: [0.450, 0.000] + size_xy_m: [1.200, 1.000] + bounds_xy_m: + x_min: -0.150 + x_max: 1.050 + y_min: -0.500 + y_max: 0.500 + notes: "Derived from confirmed table size/center in base_link." + +# Fixed dispenser-number to measured-pose mapping. +# STT/LLM may select only these IDs. Coordinates must be filled only from +# measured teaching/calibration data in base_link; never from STT, LLM, or VLA. +dispenser_outlets: + "1": + cup_pre_place_joints_deg: [11.63, 45.37, 110.46, 194.52, 63.57, -100.48] # operator measured, 2026-06-10 + cup_place_joints_deg: [10.24, 48.98, 99.43, 194.25, 55.74, -100.48] # operator measured, 2026-06-10 + 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.897411, 0.077654, 0.384322] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [3.041566, -1.889330, -2.756159] + press_pose_rpy_deg: [174.269, -108.251, -157.916] + 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_pre_joints_deg: [-4.52, 25.97, 74.19, 146.71, 10.73, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [10.80, 30.88, 57.88, -38.19, 25.14, 32.93] # operator measured, 2026-06-10 + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + clearance_m: null + "2": + cup_pre_place_joints_deg: [-11.43, 45.17, 110.50, 165.41, 64.36, -85.88] # operator measured, 2026-06-10 + cup_place_joints_deg: [-9.99, 48.08, 99.33, 165.41, 56.24, -85.88] # operator measured, 2026-06-10 + 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.894031, 0.023056, 0.383725] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [0.006618, 1.749985, 0.448575] + press_pose_rpy_deg: [0.379, 100.267, 25.701] + 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_pre_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + press_contact_joints_deg: [1.90, 27.14, 71.09, -27.11, 2.29, 23.18] # operator measured, 2026-06-10 + clearance_m: null + press_contact_status: measured_confirmed # operator measured, 2026-06-10 + "3": + cup_pre_place_joints_deg: [-3.09, 44.18, 111.56, 176.17, 62.61, -89.23] # operator measured, 2026-06-10 + cup_place_joints_deg: [1.45, 47.74, 101.87, 177.95, 57.30, -93.60] # operator measured, 2026-06-10 + 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.669475, -0.023767, 0.415318] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [0.007526, 1.704437, -3.116764] + press_pose_rpy_deg: [0.431, 97.657, -178.577] + 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_pre_joints_deg: [-4.25, 28.22, 75.41, 146.71, 10.23, -141.99] # operator measured, 2026-06-10 + press_contact_joints_deg: [-8.05, 26.38, 71.96, 146.62, 8.13, -141.99] # operator measured PRESS3, 2026-06-10 + clearance_m: null + "4": + cup_pre_place_joints_deg: [5.72, 27.39, 65.56, 167.98, -4.74, -161.56] # operator measured, 2026-06-10 + cup_place_joints_deg: [5.62, 28.54, 70.28, 167.98, -1.51, -161.56] # operator measured, 2026-06-10 + 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.660295, -0.083131, 0.405622] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [0.008298, 1.664692, -3.119918] + press_pose_rpy_deg: [0.475, 95.380, -178.758] + 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_pre_joints_deg: [-6.60, 26.79, 73.27, -40.78, -1.97, 38.55] # operator measured, 2026-06-10 + press_contact_joints_deg: [-6.28, 25.63, 70.11, -40.75, 0.09, 38.55] # operator measured PRESS4, 2026-06-10 + clearance_m: null + +cup_holder: + parent_frame: base_link + # Measured with the active Doosan TCP placed in the holder hole. + bottom_insert_center_pose_xyz_m: [0.429831, 0.220885, 0.017898] + bottom_insert_center_pose_rpy_rad: [2.513735, -3.122200, 3.038293] + top_center_estimated_xyz_m: [0.429831, 0.220885, 0.079500] + radius_m: 0.045 + height_m: 0.062 + side_grip_place: + pre_place_pose_xyz_m: [0.425545, 0.251832, 0.217371] + pre_place_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + place_final_pose_xyz_m: [0.425545, 0.251832, 0.117371] + place_final_pose_rpy_rad: [1.600936, 1.622299, 2.132633] + retreat_pose_xyz_m: [0.415096, 0.189680, 0.113925] + retreat_pose_rpy_rad: [1.608926, 1.585807, 2.146282] + approach_lift_m: 0.100 + solution_space: 2.0 + status: measured_draft + +# Dry-run-only example values. +# These values document the shape of a temporary static TF for simulation or +# TF debugging. They are not calibration data and must not be copied into the +# real hand_eye/outlet fields or used on the real robot. +example_static_tf_do_not_use_on_real_robot: + parent_frame: base_link + child_frame: camera_color_optical_frame + xyz_m: [0.35, 0.0, 0.65] + rpy_rad: [0.0, 0.0, 0.0] + example_command: "ros2 run tf2_ros static_transform_publisher --x 0.35 --y 0.0 --z 0.65 --roll 0.0 --pitch 0.0 --yaw 0.0 --frame-id base_link --child-frame-id camera_color_optical_frame" diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index c2082e3..867b7cc 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -1,162 +1,84 @@ # 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. +# - front_hold_poses were measured as base_link -> link_6 teaching poses. +# - raw_probe_poses for the collision box were remeasured as +# base_link -> gripper_tcp poses so the RG2 TCP offset is reflected. # - The boxes below are conservative primitive estimates for MoveIt Planning # Scene. Keep them disabled until verified in RViz against the real workcell. +# - Conflict resolution 2026-06-10: probe/box numbers take origin/develop's +# gripper_tcp-basis re-measurement (body box center x 0.879). The previous +# integration-branch numbers (center x 0.765) were link_6/flange-basis and +# sat ~0.11m in front of the real bottles. front_hold_poses keep this +# branch's operator teaching. metadata: frame_id: base_link measured_target_frame: link_6 + collision_probe_target_frame: gripper_tcp source: operator_teaching_tf2_echo - status: draft_probe_estimate_not_enabled - body_bottom_z_m: 0.000 + 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.020 - y: 0.020 - z: 0.020 - + x: 0.02 + y: 0.02 + z: 0.02 + nozzle_merge_policy: single horizontal spout cuboid uses the same X/Y footprint + dimensions and orientation as dispenser_combined_body_box front_hold_poses: - # Cup hold/front limit poses. Cup pose itself still comes from vision. dispenser_1: - position_xyz_m: [0.437710, 0.041751, 0.077310] - quaternion_xyzw: [0.458109, 0.520842, 0.538998, 0.477851] - rpy_deg: [87.838, 0.225, 97.099] + position_xyz_m: [0.603855, 0.083532, 0.079630] + quaternion_xyzw: [0.476935, 0.550359, 0.474620, 0.494343] + rpy_deg: [93.496, 5.244, 93.242] dispenser_2: - position_xyz_m: [0.437119, 0.007800, 0.081750] - quaternion_xyzw: [0.500394, 0.482826, 0.508624, 0.507727] - rpy_deg: [88.110, -1.074, 89.062] + position_xyz_m: [0.588709, 0.039032, 0.073706] + quaternion_xyzw: [0.506185, 0.526041, 0.482369, 0.484126] + rpy_deg: [93.778, 1.204, 91.077] dispenser_3: - position_xyz_m: [0.436445, -0.028198, 0.088599] - quaternion_xyzw: [0.526785, 0.467746, 0.481494, 0.521416] - rpy_deg: [89.575, -1.118, 84.331] + position_xyz_m: [0.607892, -0.038249, 0.083077] + quaternion_xyzw: [0.477410, 0.540579, 0.505177, 0.473973] + rpy_deg: [92.310, 1.724, 95.446] dispenser_4: - position_xyz_m: [0.437010, -0.083524, 0.087128] - quaternion_xyzw: [0.509621, 0.485335, 0.481655, 0.522249] - rpy_deg: [89.457, 0.917, 86.277] - + position_xyz_m: [0.606769, -0.093142, 0.084589] + quaternion_xyzw: [0.463470, 0.552601, 0.487955, 0.491658] + rpy_deg: [92.322, 5.226, 95.008] raw_probe_poses: - body_left_probe: - position_xyz_m: [0.533, 0.005, 0.095] - quaternion_xyzw: [0.362, 0.753, 0.526, 0.159] - rpy_deg: [113.625, -8.080, 133.982] - body_right_probe: - position_xyz_m: [0.502, -0.265, 0.095] - quaternion_xyzw: [0.403, 0.630, 0.575, 0.332] - rpy_deg: [96.862, -2.610, 117.065] - body_back_right_probe: - position_xyz_m: [0.811, -0.165, 0.120] - quaternion_xyzw: [-0.010, 0.701, 0.519, 0.489] - rpy_deg: [88.620, 44.063, 136.576] - body_top_right_probe: - position_xyz_m: [0.757, -0.184, 0.242] - quaternion_xyzw: [0.586, 0.360, 0.573, 0.446] - rpy_deg: [86.632, -20.555, 84.875] - head_top_1_probe: - position_xyz_m: [0.742, 0.066, 0.413] - quaternion_xyzw: [-0.042, 0.712, -0.047, 0.700] - rpy_deg: [-97.548, 82.646, -97.947] - head_front_probe: - position_xyz_m: [0.683, 0.029, 0.452] - quaternion_xyzw: [-0.397, 0.574, -0.374, 0.610] - rpy_deg: [-88.393, 23.817, -86.216] - + left_front_bottom_probe: + position_xyz_m: [0.850, 0.121, 0.031] + quaternion_xyzw: [0.680, -0.594, 0.283, -0.323] + rpy_deg: [-129.105, -0.054, -82.270] + right_back_top_probe: + position_xyz_m: [0.908, -0.145, 0.308] + quaternion_xyzw: [-0.527, 0.637, -0.423, 0.370] + rpy_deg: [-111.636, 1.516, -99.773] estimated_collision_objects: - dispenser_body_box: + dispenser_combined_body_box: type: box frame_id: base_link - # Legacy broad axis-aligned estimate. Kept visible as a faint comparison - # marker only because it overlaps the taught front-hold points. - center_xyz_m: [0.6815, -0.1300, 0.1310] - size_xyz_m: [0.2990, 0.3100, 0.2620] + # Single measured draft box from left-front-bottom and right-back-top + # gripper_tcp 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.8790, -0.0120, 0.1640] + size_xyz_m: [0.0980, 0.3060, 0.3280] bounds_xyz_m: - min: [0.5320, -0.2850, 0.0000] - max: [0.8310, 0.0250, 0.2620] + min: [0.8300, -0.1650, 0.0000] + max: [0.9280, 0.1410, 0.3280] orientation_xyzw: [0.0, 0.0, 0.0, 1.0] - publish_to_planning_scene: false - enabled_for_real_motion: false - dispenser_1_body_box_v2: - type: box - frame_id: base_link - # Local x axis follows the dispenser depth direction measured from the - # front-hold line toward the body/back probe. Local y follows the row. - center_xyz_m: [0.6946, -0.1393, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_2_body_box_v2: + dispenser_head_nozzle_merged_horizontal_spout_box: type: box frame_id: base_link - center_xyz_m: [0.6886, -0.1893, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_3_body_box_v2: - type: box - frame_id: base_link - center_xyz_m: [0.6666, -0.2433, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_4_body_box_v2: - type: box - frame_id: base_link - center_xyz_m: [0.6566, -0.2743, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_head_box: - type: box - frame_id: base_link - # Legacy broad head estimate. It over-blocks the real nozzle geometry: - # the measured head protrusion is about 0.05 m forward and 0.01 m wide. - # Keep this only as a disabled comparison marker. - center_xyz_m: [0.7470, -0.0995, 0.3520] - size_xyz_m: [0.1680, 0.3710, 0.2400] + center_xyz_m: [0.7810, -0.0120, 0.4320] + size_xyz_m: [0.0980, 0.3060, 0.0400] bounds_xyz_m: - min: [0.6630, -0.2850, 0.2320] - max: [0.8310, 0.0860, 0.4720] + min: [0.7320, -0.1650, 0.4120] + max: [0.8300, 0.1410, 0.4520] orientation_xyzw: [0.0, 0.0, 0.0, 1.0] - publish_to_planning_scene: false - enabled_for_real_motion: false - dispenser_1_head_nozzle_box: - type: box - frame_id: base_link - # Per-nozzle estimate: 5 cm protrusion, 1 cm row width. - center_xyz_m: [0.5640, -0.1025, 0.4320] - size_xyz_m: [0.0500, 0.0100, 0.0400] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_2_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: [0.5580, -0.1525, 0.4320] - size_xyz_m: [0.0500, 0.0100, 0.0400] - 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.5360, -0.2065, 0.4320] - size_xyz_m: [0.0500, 0.0100, 0.0400] - 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.5260, -0.2375, 0.4320] - size_xyz_m: [0.0500, 0.0100, 0.0400] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] publish_to_planning_scene: true enabled_for_real_motion: false + '# 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. Recomputed 2026-06-10 against the gripper_tcp-basis + body box (min X 0.830); verify in RViz before enabling for real motion. diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110911 b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110911 new file mode 100644 index 0000000..e7470ab --- /dev/null +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110911 @@ -0,0 +1,157 @@ +metadata: + frame_id: base_link + measured_target_frame: link_6 + source: operator_teaching_tf2_echo + 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_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: + - 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.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.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.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 + right_back_top_probe: + 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 + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 + bounds_xyz_m: + 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_head_nozzle_merged_horizontal_spout_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.721 + - -0.0155 + - 0.432 + size_xyz_m: + - 0.044 + - 0.215 + - 0.04 + bounds_xyz_m: + min: + - 0.699 + - -0.123 + - 0.412 + max: + - 0.743 + - 0.092 + - 0.452 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# 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/config/measured_dispenser_collision.yaml.bak-20260610-110944 b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110944 new file mode 100644 index 0000000..0e63391 --- /dev/null +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110944 @@ -0,0 +1,157 @@ +metadata: + frame_id: base_link + measured_target_frame: link_6 + source: operator_teaching_tf2_echo + 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_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: [0.603855, 0.083532, 0.079630] + quaternion_xyzw: [0.476935, 0.550359, 0.474620, 0.494343] + rpy_deg: [93.496, 5.244, 93.242] + - 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.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.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.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 + right_back_top_probe: + 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 + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 + bounds_xyz_m: + 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_head_nozzle_merged_horizontal_spout_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.721 + - -0.0155 + - 0.432 + size_xyz_m: + - 0.044 + - 0.215 + - 0.04 + bounds_xyz_m: + min: + - 0.699 + - -0.123 + - 0.412 + max: + - 0.743 + - 0.092 + - 0.452 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# 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/config/measured_dispenser_collision.yaml.bak-20260610-111009 b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111009 new file mode 100644 index 0000000..99ce5f5 --- /dev/null +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111009 @@ -0,0 +1,157 @@ +metadata: + frame_id: base_link + measured_target_frame: link_6 + source: operator_teaching_tf2_echo + 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_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: [0.603855, 0.083532, 0.079630] + quaternion_xyzw: [0.476935, 0.550359, 0.474620, 0.494343] + rpy_deg: [93.496, 5.244, 93.242] + - 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.608709, 0.039032, 0.073706] + quaternion_xyzw: [0.506185, 0.526041, 0.482369, 0.484126] + rpy_deg: [93.778, 1.204, 91.077] + - 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.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.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 + right_back_top_probe: + 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 + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 + bounds_xyz_m: + 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_head_nozzle_merged_horizontal_spout_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.721 + - -0.0155 + - 0.432 + size_xyz_m: + - 0.044 + - 0.215 + - 0.04 + bounds_xyz_m: + min: + - 0.699 + - -0.123 + - 0.412 + max: + - 0.743 + - 0.092 + - 0.452 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# 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/config/measured_dispenser_collision.yaml.bak-20260610-111027 b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111027 new file mode 100644 index 0000000..7801068 --- /dev/null +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111027 @@ -0,0 +1,157 @@ +metadata: + frame_id: base_link + measured_target_frame: link_6 + source: operator_teaching_tf2_echo + 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_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: [0.603855, 0.083532, 0.079630] + quaternion_xyzw: [0.476935, 0.550359, 0.474620, 0.494343] + rpy_deg: [93.496, 5.244, 93.242] + - 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.608709, 0.039032, 0.073706] + quaternion_xyzw: [0.506185, 0.526041, 0.482369, 0.484126] + rpy_deg: [93.778, 1.204, 91.077] + - 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.607892, -0.038249, 0.083077] + quaternion_xyzw: [0.477410, 0.540579, 0.505177, 0.473973] + rpy_deg: [92.310, 1.724, 95.446] + - 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.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 + right_back_top_probe: + 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 + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 + bounds_xyz_m: + 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_head_nozzle_merged_horizontal_spout_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.721 + - -0.0155 + - 0.432 + size_xyz_m: + - 0.044 + - 0.215 + - 0.04 + bounds_xyz_m: + min: + - 0.699 + - -0.123 + - 0.412 + max: + - 0.743 + - 0.092 + - 0.452 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# 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/config/safety.yaml b/src/azas_bringup/config/safety.yaml index 94d0b42..4194897 100644 --- a/src/azas_bringup/config/safety.yaml +++ b/src/azas_bringup/config/safety.yaml @@ -1,8 +1,16 @@ motion: max_velocity_scale: 0.1 # 초기 dry-run 안전 제한; 실제 운영 전 확인 필요 max_acceleration_scale: 0.1 # 초기 dry-run 안전 제한; 실제 운영 전 확인 필요 - workspace_bounds_m: null # 확인 필요 - min_z_m: null # 확인 필요: table/collision geometry 기반 + # 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 # 확인 필요 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..003c1db --- /dev/null +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -0,0 +1,167 @@ +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="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"), + DeclareLaunchArgument("classifier_min_confidence", default_value="0.70"), + DeclareLaunchArgument("route_timeout_sec", default_value="30.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"), + 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"), + 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_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"), + 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 " + "--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"), + 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", + 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"), + "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"), + "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"), + "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, + ), + "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"), + "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), + "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"), + "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_bringup/launch/cocktail_dryrun.launch.py b/src/azas_bringup/launch/cocktail_dryrun.launch.py index 22ac276..2521604 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"), @@ -54,7 +70,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"), @@ -65,6 +86,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", @@ -75,6 +97,7 @@ def generate_launch_description(): "max_detection_age_s": ParameterValue( LaunchConfiguration("max_detection_age_s"), value_type=float ), + "decision_topic": "/azas/voice/confirmed_recipe_decision", "require_cup": ParameterValue(LaunchConfiguration("require_cup"), value_type=bool), "require_lid": ParameterValue(LaunchConfiguration("require_lid"), value_type=bool), } 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 new file mode 100644 index 0000000..de82aa2 --- /dev/null +++ b/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py @@ -0,0 +1,116 @@ +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("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"), + 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.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"), + "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, + { + "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), + "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_cup_target_then_shake.launch.py b/src/azas_bringup/launch/doosan_moveit_cup_target_then_shake.launch.py index 298eedd..583a2fe 100644 --- a/src/azas_bringup/launch/doosan_moveit_cup_target_then_shake.launch.py +++ b/src/azas_bringup/launch/doosan_moveit_cup_target_then_shake.launch.py @@ -24,6 +24,10 @@ def generate_launch_description(): shake_cycles = LaunchConfiguration("shake_cycles") collision_config_path = LaunchConfiguration("collision_config_path") collision_publish_period_sec = LaunchConfiguration("collision_publish_period_sec") + safety_config_path = LaunchConfiguration("safety_config_path") + workspace_collision_publish_period_sec = LaunchConfiguration( + "workspace_collision_publish_period_sec" + ) doosan_moveit = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -159,6 +163,25 @@ def generate_launch_description(): ], ) + workspace_collision_scene = Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + parameters=[ + { + "safety_config_path": ParameterValue( + safety_config_path, + value_type=str, + ), + "publish_period_sec": ParameterValue( + workspace_collision_publish_period_sec, + value_type=float, + ), + } + ], + ) + return LaunchDescription( [ DeclareLaunchArgument("mode", default_value="virtual"), @@ -213,6 +236,13 @@ def generate_launch_description(): DeclareLaunchArgument("max_velocity_scaling_factor", default_value="0.10"), DeclareLaunchArgument("max_acceleration_scaling_factor", default_value="0.10"), DeclareLaunchArgument("controller_action_wait_sec", default_value="20.0"), + DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "safety.yaml"] + ), + ), + DeclareLaunchArgument("workspace_collision_publish_period_sec", default_value="2.0"), DeclareLaunchArgument( "collision_config_path", default_value=PathJoinSubstitution( @@ -225,6 +255,7 @@ def generate_launch_description(): ), DeclareLaunchArgument("collision_publish_period_sec", default_value="2.0"), measured_collision_scene, + workspace_collision_scene, doosan_moveit, TimerAction(period=start_delay_sec, actions=[executor]), ] diff --git a/src/azas_bringup/launch/doosan_moveit_grasped_tumbler_to_dispenser.launch.py b/src/azas_bringup/launch/doosan_moveit_grasped_tumbler_to_dispenser.launch.py index 796a3fd..9bead5b 100644 --- a/src/azas_bringup/launch/doosan_moveit_grasped_tumbler_to_dispenser.launch.py +++ b/src/azas_bringup/launch/doosan_moveit_grasped_tumbler_to_dispenser.launch.py @@ -18,6 +18,10 @@ def generate_launch_description(): start_delay_sec = LaunchConfiguration("start_delay_sec") collision_config_path = LaunchConfiguration("collision_config_path") collision_publish_period_sec = LaunchConfiguration("collision_publish_period_sec") + safety_config_path = LaunchConfiguration("safety_config_path") + workspace_collision_publish_period_sec = LaunchConfiguration( + "workspace_collision_publish_period_sec" + ) doosan_moveit = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -145,6 +149,26 @@ def generate_launch_description(): ], ) + workspace_collision_scene = Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + condition=IfCondition(LaunchConfiguration("enable_workspace_collision_scene")), + parameters=[ + { + "safety_config_path": ParameterValue( + safety_config_path, + value_type=str, + ), + "publish_period_sec": ParameterValue( + workspace_collision_publish_period_sec, + value_type=float, + ), + } + ], + ) + return LaunchDescription( [ DeclareLaunchArgument("mode", default_value="virtual"), @@ -190,6 +214,14 @@ def generate_launch_description(): DeclareLaunchArgument("max_velocity_scaling_factor", default_value="0.10"), DeclareLaunchArgument("max_acceleration_scaling_factor", default_value="0.10"), DeclareLaunchArgument("max_single_segment_joint_motion_deg", default_value="170.0"), + DeclareLaunchArgument("enable_workspace_collision_scene", default_value="true"), + DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "safety.yaml"] + ), + ), + DeclareLaunchArgument("workspace_collision_publish_period_sec", default_value="2.0"), DeclareLaunchArgument("enable_measured_collision_scene", default_value="true"), DeclareLaunchArgument( "collision_config_path", @@ -203,6 +235,7 @@ def generate_launch_description(): ), DeclareLaunchArgument("collision_publish_period_sec", default_value="1.0"), doosan_moveit, + workspace_collision_scene, measured_collision_scene, TimerAction(period=start_delay_sec, actions=[executor]), ] 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..c03e9c7 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,30 @@ def rviz_node_function(context): def generate_launch_description(): + 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", + ]) + ]) + ) + return LaunchDescription( [ DeclareLaunchArgument("model", default_value="m0609"), + collision_scene, + gripper_tcp_tree, OpaqueFunction(function=rviz_node_function), ] ) diff --git a/src/azas_bringup/launch/hardware_free_demo.launch.py b/src/azas_bringup/launch/hardware_free_demo.launch.py index 455e77b..76ed6c4 100644 --- a/src/azas_bringup/launch/hardware_free_demo.launch.py +++ b/src/azas_bringup/launch/hardware_free_demo.launch.py @@ -24,6 +24,14 @@ def generate_launch_description(): show_dispenser_markers = LaunchConfiguration("show_dispenser_markers") show_animated_cup = LaunchConfiguration("show_animated_cup") show_demo_arm = LaunchConfiguration("show_demo_arm") + show_workspace_safety = LaunchConfiguration("show_workspace_safety") + show_measured_dispenser_collision = LaunchConfiguration("show_measured_dispenser_collision") + show_full_collision_scene = LaunchConfiguration("show_full_collision_scene") + show_link6_gripper = LaunchConfiguration("show_link6_gripper") + enable_rule_joint_preview = LaunchConfiguration("enable_rule_joint_preview") + safety_config_path = LaunchConfiguration("safety_config_path") + dispenser_collision_config_path = LaunchConfiguration("dispenser_collision_config_path") + calibration_path = LaunchConfiguration("calibration_path") robot_description = { "robot_description": Command( @@ -69,6 +77,33 @@ def generate_launch_description(): DeclareLaunchArgument("show_dispenser_markers", default_value="true"), DeclareLaunchArgument("show_animated_cup", default_value="true"), DeclareLaunchArgument("show_demo_arm", default_value="true"), + DeclareLaunchArgument("show_workspace_safety", default_value="true"), + DeclareLaunchArgument("show_measured_dispenser_collision", default_value="true"), + DeclareLaunchArgument("show_full_collision_scene", default_value="true"), + DeclareLaunchArgument("show_link6_gripper", default_value="true"), + DeclareLaunchArgument("enable_rule_joint_preview", default_value="false"), + DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "safety.yaml"] + ), + ), + DeclareLaunchArgument( + "dispenser_collision_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("azas_bringup"), + "config", + "measured_dispenser_collision.yaml", + ] + ), + ), + DeclareLaunchArgument( + "calibration_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "calibration.yaml"] + ), + ), DeclareLaunchArgument("planning_group", default_value="manipulator"), DeclareLaunchArgument("ee_link", default_value="tool0"), DeclareLaunchArgument("planning_pipeline", default_value="pilz_industrial_motion_planner"), @@ -80,6 +115,8 @@ def generate_launch_description(): DeclareLaunchArgument("preview_publish_rate", default_value="30.0"), DeclareLaunchArgument("preview_frames_per_step", default_value="45"), DeclareLaunchArgument("preview_hold_frames", default_value="10"), + DeclareLaunchArgument("ik_preview_pose_limit", default_value="15"), + DeclareLaunchArgument("ik_preview_planning_start_delay_sec", default_value="2.5"), DeclareLaunchArgument( "rviz_config", default_value=PathJoinSubstitution( @@ -162,6 +199,10 @@ def generate_launch_description(): { "cup_pose_topic": tumbler_pose_topic, "frame_id": frame_id, + "calibration_path": ParameterValue(calibration_path, value_type=str), + "dispenser_collision_config_path": ParameterValue( + dispenser_collision_config_path, value_type=str + ), "selected_dispenser_id": ParameterValue( selected_dispenser_id, value_type=int ), @@ -176,6 +217,92 @@ def generate_launch_description(): } ], ), + Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + condition=IfCondition(show_workspace_safety), + parameters=[ + { + "safety_config_path": ParameterValue(safety_config_path, value_type=str), + "publish_collision_objects": True, + "publish_period_sec": 2.0, + "frame_id": frame_id, + "table_collision_enabled": True, + "table_collision_expand_to_workspace_walls": True, + "workspace_boundary_collision_enabled": True, + } + ], + ), + Node( + package="azas_motion", + executable="measured_dispenser_collision_scene_node", + name="measured_dispenser_collision_scene_node", + output="screen", + condition=IfCondition(show_measured_dispenser_collision), + parameters=[ + { + "config_path": ParameterValue( + dispenser_collision_config_path, value_type=str + ), + "calibration_path": ParameterValue(calibration_path, value_type=str), + "publish_collision_objects": True, + "publish_markers": True, + "publish_rviz_visual_tools_compat": True, + "publish_debug_labels": True, + "publish_period_sec": 2.0, + } + ], + ), + Node( + package="azas_bringup", + executable="collision_scene_rviz_publisher", + name="collision_scene_rviz_publisher", + output="screen", + condition=IfCondition(show_full_collision_scene), + parameters=[ + { + "frame_id": frame_id, + "safety_config_path": ParameterValue(safety_config_path, value_type=str), + "dispenser_collision_config_path": ParameterValue( + dispenser_collision_config_path, value_type=str + ), + "calibration_path": ParameterValue(calibration_path, value_type=str), + } + ], + ), + Node( + package="azas_motion", + executable="link6_gripper_collision_node", + name="link6_gripper_collision_node", + output="screen", + condition=IfCondition(show_link6_gripper), + parameters=[ + { + "attached_link_name": "link_6", + "publish_markers": True, + "marker_topic": "/azas/link6_gripper/markers", + "remove_on_shutdown": False, + } + ], + ), + Node( + package="azas_bringup", + executable="rule_motion_joint_preview_node", + name="rule_motion_joint_preview_node", + output="screen", + condition=IfCondition(enable_rule_joint_preview), + parameters=[ + { + "path_topic": "/azas/dispenser_sequence/plan", + "joint_state_topic": "/joint_states", + "publish_rate_hz": 30.0, + "frames_per_pose": 12, + "loop_preview": True, + } + ], + ), Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -217,6 +344,13 @@ def generate_launch_description(): "loop_preview": ParameterValue( LaunchConfiguration("loop_preview"), value_type=bool ), + "max_preview_poses": ParameterValue( + LaunchConfiguration("ik_preview_pose_limit"), value_type=int + ), + "planning_start_delay_sec": ParameterValue( + LaunchConfiguration("ik_preview_planning_start_delay_sec"), + value_type=float, + ), } ], condition=IfCondition(enable_ik_preview), diff --git a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py new file mode 100644 index 0000000..42a5959 --- /dev/null +++ b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py @@ -0,0 +1,880 @@ +import os +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +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 default_model_path(): + env_path = os.environ.get("AZAS_YOLO_MODEL_PATH") or os.environ.get("MODEL_PATH") + candidates = [ + env_path, + "/home/ssu/Downloads/best.pt", + str( + Path(__file__).resolve().parents[3] + / "src" + / "cocktail_robot_system" + / "models" + / "best.pt" + ), + "/home/ssu/ros2_ws/src/cocktail_robot_system/src/cocktail_robot_system/models/best.pt", + ] + for candidate in candidates: + if candidate and Path(candidate).is_file(): + return str(candidate) + return "/home/ssu/Downloads/best.pt" + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument("model_path", default_value=default_model_path()), + 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"), + DeclareLaunchArgument("confidence_threshold", default_value="0.35"), + DeclareLaunchArgument("target_class_names", default_value="lid"), + DeclareLaunchArgument("selection_policy", default_value="highest_confidence"), + DeclareLaunchArgument("source_frame", default_value="camera_color_optical_frame"), + DeclareLaunchArgument("target_frame", default_value="base_link"), + DeclareLaunchArgument("require_tf", default_value="true"), + DeclareLaunchArgument("transform_timeout_sec", default_value="0.2"), + DeclareLaunchArgument("allow_latest_tf_fallback", default_value="true"), + DeclareLaunchArgument("publish_hand_eye_tf", default_value="true"), + DeclareLaunchArgument( + "hand_eye_matrix_path", + default_value=PathJoinSubstitution([ + FindPackageShare("azas_perception"), + "config", + "T_gripper2camera.npy", + ]), + ), + DeclareLaunchArgument("hand_eye_parent_frame", default_value="link_6"), + DeclareLaunchArgument("hand_eye_matrix_child_frame", default_value="camera_color_optical_frame"), + DeclareLaunchArgument("hand_eye_published_child_frame", default_value="camera_link"), + DeclareLaunchArgument("hand_eye_translation_scale", default_value="0.001"), + DeclareLaunchArgument("hand_eye_compose_with_existing_tf", default_value="true"), + DeclareLaunchArgument("hand_eye_compose_timeout_sec", default_value="5.0"), + DeclareLaunchArgument("device", default_value="cpu"), + DeclareLaunchArgument("show_preview", default_value="true"), + DeclareLaunchArgument("log_detections", default_value="false"), + DeclareLaunchArgument("log_bridge_poses", default_value="false"), + 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"), + DeclareLaunchArgument("approach_pose_topic", default_value="/jarvis/lid_gripper/approach_pose"), + DeclareLaunchArgument("grasp_pose_topic", default_value="/jarvis/lid_gripper/grasp_pose"), + DeclareLaunchArgument("lift_pose_topic", default_value="/jarvis/lid_gripper/lift_pose"), + DeclareLaunchArgument("status_topic", default_value="/jarvis/lid_gripper/status"), + DeclareLaunchArgument("plan_on_pose", default_value="true"), + DeclareLaunchArgument("depth_window_size", default_value="7"), + DeclareLaunchArgument("min_depth_m", default_value="0.15"), + DeclareLaunchArgument("max_depth_m", default_value="2.0"), + DeclareLaunchArgument("marker_type", default_value="aruco"), + DeclareLaunchArgument("require_lid_detection", default_value="true"), + DeclareLaunchArgument("allow_aruco_only_after_grip_request", default_value="true"), + DeclareLaunchArgument("aruco_only_after_grip_request_sec", default_value="20.0"), + DeclareLaunchArgument("roi_padding_ratio", default_value="0.12"), + 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_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"), + DeclareLaunchArgument("plane_patch_radius_px", default_value="18"), + DeclareLaunchArgument("min_plane_points", default_value="20"), + DeclareLaunchArgument("max_plane_rmse_m", default_value="0.015"), + DeclareLaunchArgument("approach_offset_m", default_value="0.08"), + DeclareLaunchArgument("lift_offset_m", default_value="0.10"), + DeclareLaunchArgument("min_approach_z_m", default_value="0.0"), + DeclareLaunchArgument("surface_offset_m", default_value="0.0"), + DeclareLaunchArgument("offset_axis", default_value="local_z"), + DeclareLaunchArgument("tcp_grasp_offset_x_m", default_value="0.0"), + DeclareLaunchArgument("tcp_grasp_offset_y_m", default_value="0.0"), + DeclareLaunchArgument("tcp_grasp_offset_z_m", default_value="0.0"), + DeclareLaunchArgument("min_grasp_z_m", default_value="0.02"), + DeclareLaunchArgument("max_grasp_z_m", default_value="0.60"), + DeclareLaunchArgument("enable_hardware", default_value="false"), + DeclareLaunchArgument("hardware_confirm", default_value=""), + DeclareLaunchArgument("allow_service_control_without_moveit", default_value="false"), + DeclareLaunchArgument("service_prefix", default_value=""), + DeclareLaunchArgument("rx", default_value="180.0"), + DeclareLaunchArgument("ry", default_value="0.0"), + DeclareLaunchArgument("rz", default_value="180.0"), + DeclareLaunchArgument("use_lid_pose_yaw_for_pick", default_value="false"), + 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"), + 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"), + DeclareLaunchArgument("verify_motion_reached", default_value="true"), + DeclareLaunchArgument("motion_verify_timeout_sec", default_value="20.0"), + DeclareLaunchArgument("motion_target_tolerance_m", default_value="0.015"), + DeclareLaunchArgument("visual_refine_before_grasp", default_value="false"), + DeclareLaunchArgument("visual_refine_sample_count", default_value="10"), + DeclareLaunchArgument("visual_refine_timeout_sec", default_value="2.0"), + DeclareLaunchArgument("visual_refine_min_sample_interval_sec", default_value="0.03"), + DeclareLaunchArgument("visual_refine_max_yaw_std_deg", default_value="3.0"), + DeclareLaunchArgument("visual_refine_max_position_std_m", default_value="0.005"), + DeclareLaunchArgument("visual_refine_apply_xy", default_value="true"), + DeclareLaunchArgument("visual_refine_apply_yaw", default_value="true"), + DeclareLaunchArgument("visual_refine_fallback_to_initial_plan", default_value="true"), + DeclareLaunchArgument("settle_seconds_before_grasp", default_value="0.5"), + DeclareLaunchArgument("hold_seconds_after_grasp", default_value="0.2"), + DeclareLaunchArgument("enable_lid_twist_after_grasp", default_value="false"), + DeclareLaunchArgument("lid_twist_target_x_m", default_value="nan"), + DeclareLaunchArgument("lid_twist_target_y_m", default_value="nan"), + DeclareLaunchArgument("lid_twist_target_z_m", default_value="nan"), + DeclareLaunchArgument("lid_twist_rx", default_value="nan"), + DeclareLaunchArgument("lid_twist_ry", default_value="nan"), + DeclareLaunchArgument("lid_twist_rz", default_value="nan"), + DeclareLaunchArgument("lid_twist_use_force_control", default_value="false"), + DeclareLaunchArgument("lid_twist_use_force_spiral", default_value="false"), + DeclareLaunchArgument("lid_twist_press_down_m", default_value="0.0"), + DeclareLaunchArgument("lid_twist_rz_delta_deg", default_value="-30.0"), + DeclareLaunchArgument("lid_twist_turn_step_deg", default_value="90.0"), + DeclareLaunchArgument("lid_twist_transfer_clearance_m", default_value="0.0"), + DeclareLaunchArgument("lid_twist_release_lift_m", default_value="0.03"), + DeclareLaunchArgument("lid_twist_min_z_m", default_value="0.02"), + DeclareLaunchArgument("lid_twist_max_z_m", default_value="0.60"), + DeclareLaunchArgument("lid_twist_transfer_max_z_m", default_value="0.60"), + DeclareLaunchArgument("lid_twist_transfer_velocity", default_value="30.0"), + DeclareLaunchArgument("lid_twist_press_velocity", default_value="8.0"), + DeclareLaunchArgument("lid_twist_turn_velocity", default_value="8.0"), + DeclareLaunchArgument("lid_twist_acceleration", default_value="10.0"), + DeclareLaunchArgument("lid_twist_hold_seconds_before_turn", default_value="0.3"), + DeclareLaunchArgument("lid_twist_hold_seconds_after_turn", default_value="0.5"), + DeclareLaunchArgument("lid_twist_down_force_n", default_value="8.0"), + DeclareLaunchArgument("lid_twist_force_ref", default_value="base"), + DeclareLaunchArgument("lid_twist_force_rotation_mode", default_value="movel"), + DeclareLaunchArgument("lid_twist_force_service_timeout_sec", default_value="5.0"), + DeclareLaunchArgument("lid_twist_regrip_cycles", default_value="1"), + DeclareLaunchArgument("lid_twist_regrip_turn_deg", default_value="nan"), + DeclareLaunchArgument("lid_twist_regrip_reset_between_cycles", default_value="false"), + 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"), + DeclareLaunchArgument("lid_twist_preseat_periodic_z_amp_mm", default_value="1.0"), + DeclareLaunchArgument("lid_twist_preseat_periodic_rx_amp_deg", default_value="0.0"), + DeclareLaunchArgument("lid_twist_preseat_periodic_ry_amp_deg", default_value="3.0"), + DeclareLaunchArgument("lid_twist_preseat_periodic_rz_amp_deg", default_value="5.0"), + DeclareLaunchArgument("lid_twist_preseat_periodic_period_sec", default_value="1.0"), + 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"), + DeclareLaunchArgument("lid_twist_compliance_rx_stiffness", default_value="200.0"), + DeclareLaunchArgument("lid_twist_compliance_ry_stiffness", default_value="200.0"), + DeclareLaunchArgument("lid_twist_compliance_rz_stiffness", default_value="200.0"), + DeclareLaunchArgument("motion_target_orientation_tolerance_deg", default_value="3.0"), + DeclareLaunchArgument("enable_gripper_service_calls", default_value="false"), + DeclareLaunchArgument("execute_gripper_on_pose", default_value="false"), + DeclareLaunchArgument("gripper_set_service", default_value="/jarvis/rg2/set_width"), + DeclareLaunchArgument("gripper_preopen_width_m", default_value="nan"), + DeclareLaunchArgument("gripper_grasp_width_m", default_value="nan"), + DeclareLaunchArgument("gripper_force_n", default_value="nan"), + DeclareLaunchArgument("gripper_wait_timeout_sec", default_value="10.0"), + DeclareLaunchArgument("continue_after_gripper_grasp_failure", default_value="false"), + DeclareLaunchArgument("gripper_grasp_failure_wait_sec", default_value="2.0"), + Node( + package="azas_perception", + executable="hand_eye_static_tf_node", + name="hand_eye_static_tf_node", + output="screen", + condition=IfCondition(LaunchConfiguration("publish_hand_eye_tf")), + parameters=[{ + "matrix_path": LaunchConfiguration("hand_eye_matrix_path"), + "parent_frame": LaunchConfiguration("hand_eye_parent_frame"), + "matrix_child_frame": LaunchConfiguration("hand_eye_matrix_child_frame"), + "published_child_frame": LaunchConfiguration("hand_eye_published_child_frame"), + "translation_scale": ParameterValue( + LaunchConfiguration("hand_eye_translation_scale"), + value_type=float, + ), + "compose_with_existing_tf": ParameterValue( + LaunchConfiguration("hand_eye_compose_with_existing_tf"), + value_type=bool, + ), + "compose_timeout_sec": ParameterValue( + LaunchConfiguration("hand_eye_compose_timeout_sec"), + value_type=float, + ), + }], + ), + Node( + package="azas_perception", + executable="lid_sticker_detector_node", + name="lid_sticker_detector_node", + output="screen", + parameters=[{ + "model_path": LaunchConfiguration("model_path"), + "color_topic": LaunchConfiguration("color_topic"), + "depth_topic": LaunchConfiguration("depth_topic"), + "camera_info_topic": LaunchConfiguration("camera_info_topic"), + "output_topic": LaunchConfiguration("lid_detection_topic"), + "grip_request_topic": LaunchConfiguration("grip_request_topic"), + "confidence_threshold": ParameterValue( + LaunchConfiguration("confidence_threshold"), + value_type=float, + ), + "target_class_names": LaunchConfiguration("target_class_names"), + "selection_policy": LaunchConfiguration("selection_policy"), + "source_frame": LaunchConfiguration("source_frame"), + "depth_window_size": ParameterValue( + LaunchConfiguration("depth_window_size"), + value_type=int, + ), + "min_depth_m": ParameterValue(LaunchConfiguration("min_depth_m"), value_type=float), + "max_depth_m": ParameterValue(LaunchConfiguration("max_depth_m"), value_type=float), + "marker_type": LaunchConfiguration("marker_type"), + "require_lid_detection": ParameterValue( + LaunchConfiguration("require_lid_detection"), + value_type=bool, + ), + "allow_aruco_only_after_grip_request": ParameterValue( + LaunchConfiguration("allow_aruco_only_after_grip_request"), + value_type=bool, + ), + "aruco_only_after_grip_request_sec": ParameterValue( + LaunchConfiguration("aruco_only_after_grip_request_sec"), + value_type=float, + ), + "roi_padding_ratio": ParameterValue( + LaunchConfiguration("roi_padding_ratio"), + value_type=float, + ), + "red_min_area_px": ParameterValue( + LaunchConfiguration("red_min_area_px"), + value_type=float, + ), + "red_min_radius_px": ParameterValue( + LaunchConfiguration("red_min_radius_px"), + value_type=float, + ), + "red_min_circularity": ParameterValue( + LaunchConfiguration("red_min_circularity"), + value_type=float, + ), + "aruco_dictionary": LaunchConfiguration("aruco_dictionary"), + "aruco_marker_id": ParameterValue( + 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, + ), + "use_aruco_axis_for_orientation": ParameterValue( + LaunchConfiguration("use_aruco_axis_for_orientation"), + value_type=bool, + ), + "aruco_finger_axis_quarter_turns": ParameterValue( + LaunchConfiguration("aruco_finger_axis_quarter_turns"), + value_type=int, + ), + "plane_patch_radius_px": ParameterValue( + LaunchConfiguration("plane_patch_radius_px"), + value_type=int, + ), + "min_plane_points": ParameterValue( + LaunchConfiguration("min_plane_points"), + value_type=int, + ), + "max_plane_rmse_m": ParameterValue( + LaunchConfiguration("max_plane_rmse_m"), + value_type=float, + ), + "device": LaunchConfiguration("device"), + "log_detections": ParameterValue( + LaunchConfiguration("log_detections"), + 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( + package="azas_perception", + executable="cup_detection_pose_bridge_node", + name="lid_detection_pose_bridge_node", + output="screen", + parameters=[{ + "input_topic": LaunchConfiguration("lid_detection_topic"), + "output_topic": LaunchConfiguration("lid_pose_topic"), + "min_confidence": ParameterValue( + LaunchConfiguration("confidence_threshold"), + value_type=float, + ), + "require_status_prefix": "detected:lid", + "require_upright_status": False, + "target_frame": LaunchConfiguration("target_frame"), + "require_tf": ParameterValue(LaunchConfiguration("require_tf"), value_type=bool), + "source_frame": LaunchConfiguration("source_frame"), + "transform_timeout_sec": ParameterValue( + LaunchConfiguration("transform_timeout_sec"), + value_type=float, + ), + "tf_timeout_sec": ParameterValue( + LaunchConfiguration("transform_timeout_sec"), + value_type=float, + ), + "allow_latest_tf_fallback": ParameterValue( + LaunchConfiguration("allow_latest_tf_fallback"), + value_type=bool, + ), + "log_published_pose": ParameterValue( + LaunchConfiguration("log_bridge_poses"), + value_type=bool, + ), + }], + ), + Node( + package="azas_motion", + executable="lid_grip_planner_node", + name="lid_grip_planner_node", + output="screen", + parameters=[{ + "lid_pose_topic": LaunchConfiguration("lid_pose_topic"), + "trigger_topic": LaunchConfiguration("grip_request_topic"), + "approach_pose_topic": LaunchConfiguration("approach_pose_topic"), + "grasp_pose_topic": LaunchConfiguration("grasp_pose_topic"), + "lift_pose_topic": LaunchConfiguration("lift_pose_topic"), + "status_topic": LaunchConfiguration("status_topic"), + "plan_on_pose": ParameterValue(LaunchConfiguration("plan_on_pose"), value_type=bool), + "log_pose_plans": ParameterValue( + LaunchConfiguration("log_pose_plans"), + value_type=bool, + ), + "log_korean_status": ParameterValue( + LaunchConfiguration("log_korean_status"), + value_type=bool, + ), + "log_json_status": ParameterValue( + LaunchConfiguration("log_json_status"), + value_type=bool, + ), + "approach_offset_m": ParameterValue( + LaunchConfiguration("approach_offset_m"), + value_type=float, + ), + "lift_offset_m": ParameterValue(LaunchConfiguration("lift_offset_m"), value_type=float), + "min_approach_z_m": ParameterValue( + LaunchConfiguration("min_approach_z_m"), + value_type=float, + ), + "surface_offset_m": ParameterValue( + LaunchConfiguration("surface_offset_m"), + value_type=float, + ), + "offset_axis": ParameterValue( + LaunchConfiguration("offset_axis"), + value_type=str, + ), + "tcp_grasp_offset_x_m": ParameterValue( + LaunchConfiguration("tcp_grasp_offset_x_m"), + value_type=float, + ), + "tcp_grasp_offset_y_m": ParameterValue( + LaunchConfiguration("tcp_grasp_offset_y_m"), + value_type=float, + ), + "tcp_grasp_offset_z_m": ParameterValue( + LaunchConfiguration("tcp_grasp_offset_z_m"), + value_type=float, + ), + "min_grasp_z_m": ParameterValue(LaunchConfiguration("min_grasp_z_m"), value_type=float), + "max_grasp_z_m": ParameterValue(LaunchConfiguration("max_grasp_z_m"), value_type=float), + "enable_hardware": ParameterValue( + LaunchConfiguration("enable_hardware"), + value_type=bool, + ), + "hardware_confirm": ParameterValue( + LaunchConfiguration("hardware_confirm"), + value_type=str, + ), + "allow_service_control_without_moveit": ParameterValue( + LaunchConfiguration("allow_service_control_without_moveit"), + value_type=bool, + ), + "service_prefix": ParameterValue( + LaunchConfiguration("service_prefix"), + value_type=str, + ), + "rx": ParameterValue(LaunchConfiguration("rx"), value_type=float), + "ry": ParameterValue(LaunchConfiguration("ry"), value_type=float), + "rz": ParameterValue(LaunchConfiguration("rz"), value_type=float), + "use_lid_pose_yaw_for_pick": ParameterValue( + LaunchConfiguration("use_lid_pose_yaw_for_pick"), + value_type=bool, + ), + "lid_pose_yaw_axis": ParameterValue( + LaunchConfiguration("lid_pose_yaw_axis"), + value_type=str, + ), + "lid_pose_yaw_offset_deg": ParameterValue( + LaunchConfiguration("lid_pose_yaw_offset_deg"), + value_type=float, + ), + "lid_pose_yaw_equivalence_deg": ParameterValue( + 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, + ), + "line_acceleration": ParameterValue( + LaunchConfiguration("line_acceleration"), + value_type=float, + ), + "move_timeout_sec": ParameterValue( + 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, + ), + "ikin_sol_space": ParameterValue( + LaunchConfiguration("ikin_sol_space"), + value_type=int, + ), + "ikin_timeout_sec": ParameterValue( + LaunchConfiguration("ikin_timeout_sec"), + value_type=float, + ), + "verify_motion_reached": ParameterValue( + LaunchConfiguration("verify_motion_reached"), + value_type=bool, + ), + "motion_verify_timeout_sec": ParameterValue( + LaunchConfiguration("motion_verify_timeout_sec"), + value_type=float, + ), + "motion_target_tolerance_m": ParameterValue( + LaunchConfiguration("motion_target_tolerance_m"), + value_type=float, + ), + "visual_refine_before_grasp": ParameterValue( + LaunchConfiguration("visual_refine_before_grasp"), + value_type=bool, + ), + "visual_refine_sample_count": ParameterValue( + LaunchConfiguration("visual_refine_sample_count"), + value_type=int, + ), + "visual_refine_timeout_sec": ParameterValue( + LaunchConfiguration("visual_refine_timeout_sec"), + value_type=float, + ), + "visual_refine_min_sample_interval_sec": ParameterValue( + LaunchConfiguration("visual_refine_min_sample_interval_sec"), + value_type=float, + ), + "visual_refine_max_yaw_std_deg": ParameterValue( + LaunchConfiguration("visual_refine_max_yaw_std_deg"), + value_type=float, + ), + "visual_refine_max_position_std_m": ParameterValue( + LaunchConfiguration("visual_refine_max_position_std_m"), + value_type=float, + ), + "visual_refine_apply_xy": ParameterValue( + LaunchConfiguration("visual_refine_apply_xy"), + value_type=bool, + ), + "visual_refine_apply_yaw": ParameterValue( + LaunchConfiguration("visual_refine_apply_yaw"), + value_type=bool, + ), + "visual_refine_fallback_to_initial_plan": ParameterValue( + LaunchConfiguration("visual_refine_fallback_to_initial_plan"), + value_type=bool, + ), + "settle_seconds_before_grasp": ParameterValue( + LaunchConfiguration("settle_seconds_before_grasp"), + value_type=float, + ), + "hold_seconds_after_grasp": ParameterValue( + LaunchConfiguration("hold_seconds_after_grasp"), + value_type=float, + ), + "enable_lid_twist_after_grasp": ParameterValue( + LaunchConfiguration("enable_lid_twist_after_grasp"), + value_type=bool, + ), + "lid_twist_target_x_m": ParameterValue( + LaunchConfiguration("lid_twist_target_x_m"), + value_type=float, + ), + "lid_twist_target_y_m": ParameterValue( + LaunchConfiguration("lid_twist_target_y_m"), + value_type=float, + ), + "lid_twist_target_z_m": ParameterValue( + LaunchConfiguration("lid_twist_target_z_m"), + value_type=float, + ), + "lid_twist_rx": ParameterValue( + LaunchConfiguration("lid_twist_rx"), + value_type=float, + ), + "lid_twist_ry": ParameterValue( + LaunchConfiguration("lid_twist_ry"), + value_type=float, + ), + "lid_twist_rz": ParameterValue( + LaunchConfiguration("lid_twist_rz"), + value_type=float, + ), + "lid_twist_use_force_control": ParameterValue( + LaunchConfiguration("lid_twist_use_force_control"), + value_type=bool, + ), + "lid_twist_use_force_spiral": ParameterValue( + LaunchConfiguration("lid_twist_use_force_spiral"), + value_type=bool, + ), + "lid_twist_press_down_m": ParameterValue( + LaunchConfiguration("lid_twist_press_down_m"), + value_type=float, + ), + "lid_twist_rz_delta_deg": ParameterValue( + LaunchConfiguration("lid_twist_rz_delta_deg"), + value_type=float, + ), + "lid_twist_turn_step_deg": ParameterValue( + LaunchConfiguration("lid_twist_turn_step_deg"), + value_type=float, + ), + "lid_twist_transfer_clearance_m": ParameterValue( + LaunchConfiguration("lid_twist_transfer_clearance_m"), + value_type=float, + ), + "lid_twist_release_lift_m": ParameterValue( + LaunchConfiguration("lid_twist_release_lift_m"), + value_type=float, + ), + "lid_twist_min_z_m": ParameterValue( + LaunchConfiguration("lid_twist_min_z_m"), + value_type=float, + ), + "lid_twist_max_z_m": ParameterValue( + LaunchConfiguration("lid_twist_max_z_m"), + value_type=float, + ), + "lid_twist_transfer_max_z_m": ParameterValue( + LaunchConfiguration("lid_twist_transfer_max_z_m"), + value_type=float, + ), + "lid_twist_transfer_velocity": ParameterValue( + LaunchConfiguration("lid_twist_transfer_velocity"), + value_type=float, + ), + "lid_twist_press_velocity": ParameterValue( + LaunchConfiguration("lid_twist_press_velocity"), + value_type=float, + ), + "lid_twist_turn_velocity": ParameterValue( + LaunchConfiguration("lid_twist_turn_velocity"), + value_type=float, + ), + "lid_twist_acceleration": ParameterValue( + LaunchConfiguration("lid_twist_acceleration"), + value_type=float, + ), + "lid_twist_hold_seconds_before_turn": ParameterValue( + LaunchConfiguration("lid_twist_hold_seconds_before_turn"), + value_type=float, + ), + "lid_twist_hold_seconds_after_turn": ParameterValue( + LaunchConfiguration("lid_twist_hold_seconds_after_turn"), + value_type=float, + ), + "lid_twist_down_force_n": ParameterValue( + LaunchConfiguration("lid_twist_down_force_n"), + value_type=float, + ), + "lid_twist_force_ref": ParameterValue( + LaunchConfiguration("lid_twist_force_ref"), + value_type=str, + ), + "lid_twist_force_rotation_mode": ParameterValue( + LaunchConfiguration("lid_twist_force_rotation_mode"), + value_type=str, + ), + "lid_twist_force_service_timeout_sec": ParameterValue( + LaunchConfiguration("lid_twist_force_service_timeout_sec"), + value_type=float, + ), + "lid_twist_regrip_cycles": ParameterValue( + LaunchConfiguration("lid_twist_regrip_cycles"), + value_type=int, + ), + "lid_twist_regrip_turn_deg": ParameterValue( + LaunchConfiguration("lid_twist_regrip_turn_deg"), + value_type=float, + ), + "lid_twist_regrip_reset_between_cycles": ParameterValue( + LaunchConfiguration("lid_twist_regrip_reset_between_cycles"), + value_type=bool, + ), + "lid_twist_regrip_gripper_wait_sec": ParameterValue( + LaunchConfiguration("lid_twist_regrip_gripper_wait_sec"), + value_type=float, + ), + "lid_twist_force_settle_seconds": ParameterValue( + LaunchConfiguration("lid_twist_force_settle_seconds"), + value_type=float, + ), + "lid_twist_force_release_time": ParameterValue( + 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, + ), + "lid_twist_preseat_periodic_x_amp_mm": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_x_amp_mm"), + value_type=float, + ), + "lid_twist_preseat_periodic_y_amp_mm": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_y_amp_mm"), + value_type=float, + ), + "lid_twist_preseat_periodic_z_amp_mm": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_z_amp_mm"), + value_type=float, + ), + "lid_twist_preseat_periodic_rx_amp_deg": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_rx_amp_deg"), + value_type=float, + ), + "lid_twist_preseat_periodic_ry_amp_deg": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_ry_amp_deg"), + value_type=float, + ), + "lid_twist_preseat_periodic_rz_amp_deg": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_rz_amp_deg"), + value_type=float, + ), + "lid_twist_preseat_periodic_period_sec": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_period_sec"), + value_type=float, + ), + "lid_twist_preseat_periodic_acc_time_sec": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_acc_time_sec"), + value_type=float, + ), + "lid_twist_preseat_periodic_repeat": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_repeat"), + value_type=int, + ), + "lid_twist_preseat_periodic_ref": ParameterValue( + 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, + ), + "lid_twist_compliance_y_stiffness": ParameterValue( + LaunchConfiguration("lid_twist_compliance_y_stiffness"), + value_type=float, + ), + "lid_twist_compliance_z_stiffness": ParameterValue( + LaunchConfiguration("lid_twist_compliance_z_stiffness"), + value_type=float, + ), + "lid_twist_compliance_rx_stiffness": ParameterValue( + LaunchConfiguration("lid_twist_compliance_rx_stiffness"), + value_type=float, + ), + "lid_twist_compliance_ry_stiffness": ParameterValue( + LaunchConfiguration("lid_twist_compliance_ry_stiffness"), + value_type=float, + ), + "lid_twist_compliance_rz_stiffness": ParameterValue( + LaunchConfiguration("lid_twist_compliance_rz_stiffness"), + value_type=float, + ), + "motion_target_orientation_tolerance_deg": ParameterValue( + LaunchConfiguration("motion_target_orientation_tolerance_deg"), + value_type=float, + ), + "enable_gripper_service_calls": ParameterValue( + LaunchConfiguration("enable_gripper_service_calls"), + value_type=bool, + ), + "execute_gripper_on_pose": ParameterValue( + LaunchConfiguration("execute_gripper_on_pose"), + value_type=bool, + ), + "gripper_set_service": LaunchConfiguration("gripper_set_service"), + "gripper_preopen_width_m": ParameterValue( + LaunchConfiguration("gripper_preopen_width_m"), + value_type=float, + ), + "gripper_grasp_width_m": ParameterValue( + LaunchConfiguration("gripper_grasp_width_m"), + value_type=float, + ), + "gripper_force_n": ParameterValue( + LaunchConfiguration("gripper_force_n"), + value_type=float, + ), + "gripper_wait_timeout_sec": ParameterValue( + LaunchConfiguration("gripper_wait_timeout_sec"), + value_type=float, + ), + "continue_after_gripper_grasp_failure": ParameterValue( + LaunchConfiguration("continue_after_gripper_grasp_failure"), + value_type=bool, + ), + "gripper_grasp_failure_wait_sec": ParameterValue( + LaunchConfiguration("gripper_grasp_failure_wait_sec"), + value_type=float, + ), + }], + ), + ]) diff --git a/src/azas_bringup/launch/measured_joint_preview_display.launch.py b/src/azas_bringup/launch/measured_joint_preview_display.launch.py new file mode 100644 index 0000000..8437f56 --- /dev/null +++ b/src/azas_bringup/launch/measured_joint_preview_display.launch.py @@ -0,0 +1,174 @@ +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 +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + frame_id = LaunchConfiguration("frame_id") + robot_color = LaunchConfiguration("robot_color") + use_rviz = LaunchConfiguration("use_rviz") + rviz_config = LaunchConfiguration("rviz_config") + show_workspace_safety = LaunchConfiguration("show_workspace_safety") + show_measured_dispenser_collision = LaunchConfiguration("show_measured_dispenser_collision") + show_full_collision_scene = LaunchConfiguration("show_full_collision_scene") + show_link6_gripper = LaunchConfiguration("show_link6_gripper") + publish_workspace_collision_objects = LaunchConfiguration("publish_workspace_collision_objects") + publish_dispenser_collision_objects = LaunchConfiguration("publish_dispenser_collision_objects") + safety_config_path = LaunchConfiguration("safety_config_path") + dispenser_collision_config_path = LaunchConfiguration("dispenser_collision_config_path") + calibration_path = LaunchConfiguration("calibration_path") + + robot_description = { + "robot_description": ParameterValue( + Command( + [ + FindExecutable(name="xacro"), + " ", + PathJoinSubstitution( + [FindPackageShare("dsr_description2"), "xacro", "m0609.urdf.xacro"] + ), + " color:=", + robot_color, + " simple:=true", + ] + ), + value_type=str, + ) + } + + return LaunchDescription( + [ + DeclareLaunchArgument("frame_id", default_value="base_link"), + DeclareLaunchArgument("robot_color", default_value="white"), + DeclareLaunchArgument("use_rviz", default_value="false"), + DeclareLaunchArgument("show_workspace_safety", default_value="true"), + DeclareLaunchArgument("show_measured_dispenser_collision", default_value="true"), + DeclareLaunchArgument("show_full_collision_scene", default_value="true"), + DeclareLaunchArgument("show_link6_gripper", default_value="true"), + DeclareLaunchArgument("publish_workspace_collision_objects", default_value="true"), + DeclareLaunchArgument("publish_dispenser_collision_objects", default_value="true"), + DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "safety.yaml"] + ), + ), + DeclareLaunchArgument( + "dispenser_collision_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("azas_bringup"), + "config", + "measured_dispenser_collision.yaml", + ] + ), + ), + DeclareLaunchArgument( + "calibration_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "calibration.yaml"] + ), + ), + DeclareLaunchArgument( + "rviz_config", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "rviz", "azas_dispenser_sequence_clean.rviz"] + ), + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="m0609_robot_state_publisher", + output="screen", + parameters=[robot_description], + ), + Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + condition=IfCondition(show_workspace_safety), + parameters=[ + { + "safety_config_path": ParameterValue(safety_config_path, value_type=str), + "publish_collision_objects": ParameterValue( + publish_workspace_collision_objects, + value_type=bool, + ), + "publish_period_sec": 2.0, + "frame_id": frame_id, + "table_collision_enabled": True, + "table_collision_expand_to_workspace_walls": True, + "workspace_boundary_collision_enabled": True, + } + ], + ), + Node( + package="azas_motion", + executable="measured_dispenser_collision_scene_node", + name="measured_dispenser_collision_scene_node", + output="screen", + condition=IfCondition(show_measured_dispenser_collision), + parameters=[ + { + "config_path": ParameterValue( + dispenser_collision_config_path, value_type=str + ), + "calibration_path": ParameterValue(calibration_path, value_type=str), + "publish_collision_objects": ParameterValue( + publish_dispenser_collision_objects, + value_type=bool, + ), + "publish_markers": True, + "publish_rviz_visual_tools_compat": False, + "publish_debug_labels": True, + "publish_period_sec": 2.0, + } + ], + ), + Node( + package="azas_bringup", + executable="collision_scene_rviz_publisher", + name="collision_scene_rviz_publisher", + output="screen", + condition=IfCondition(show_full_collision_scene), + parameters=[ + { + "frame_id": frame_id, + "safety_config_path": ParameterValue(safety_config_path, value_type=str), + "dispenser_collision_config_path": ParameterValue( + dispenser_collision_config_path, value_type=str + ), + "calibration_path": ParameterValue(calibration_path, value_type=str), + } + ], + ), + Node( + package="azas_motion", + executable="link6_gripper_collision_node", + name="link6_gripper_collision_node", + output="screen", + condition=IfCondition(show_link6_gripper), + parameters=[ + { + "attached_link_name": "link_6", + "publish_markers": True, + "marker_topic": "/azas/link6_gripper/markers", + "remove_on_shutdown": False, + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + condition=IfCondition(use_rviz), + output="screen", + ), + ] + ) 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..a268add --- /dev/null +++ b/src/azas_bringup/launch/rg2_link6_tcp.launch.py @@ -0,0 +1,51 @@ +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 +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"), + DeclareLaunchArgument("publish_gripper_collision", default_value="false"), + 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}], + ), + 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/launch/robot_connection_control.launch.py b/src/azas_bringup/launch/robot_connection_control.launch.py index 5c3283e..96f76e9 100644 --- a/src/azas_bringup/launch/robot_connection_control.launch.py +++ b/src/azas_bringup/launch/robot_connection_control.launch.py @@ -72,6 +72,7 @@ def generate_launch_description(): "transform_timeout_sec": LaunchConfiguration("transform_timeout_sec"), "debug_pose_logging": LaunchConfiguration("debug_pose_logging"), "publish_camera_base_tf": LaunchConfiguration("publish_camera_base_tf"), + "publish_hand_eye_tf": LaunchConfiguration("publish_hand_eye_tf"), "camera_base_parent_frame": LaunchConfiguration("camera_base_parent_frame"), "camera_base_child_frame": LaunchConfiguration("camera_base_child_frame"), "camera_base_tf_parent_frame": LaunchConfiguration("camera_base_tf_parent_frame"), @@ -82,12 +83,24 @@ def generate_launch_description(): "camera_base_tf_roll": LaunchConfiguration("camera_base_tf_roll"), "camera_base_tf_pitch": LaunchConfiguration("camera_base_tf_pitch"), "camera_base_tf_yaw": LaunchConfiguration("camera_base_tf_yaw"), + "hand_eye_matrix_path": LaunchConfiguration("hand_eye_matrix_path"), + "hand_eye_parent_frame": LaunchConfiguration("hand_eye_parent_frame"), + "hand_eye_matrix_child_frame": LaunchConfiguration("hand_eye_matrix_child_frame"), + "hand_eye_published_child_frame": LaunchConfiguration("hand_eye_published_child_frame"), + "hand_eye_translation_scale": LaunchConfiguration("hand_eye_translation_scale"), + "hand_eye_compose_with_existing_tf": LaunchConfiguration("hand_eye_compose_with_existing_tf"), + "hand_eye_compose_timeout_sec": LaunchConfiguration("hand_eye_compose_timeout_sec"), }.items(), ) 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"), @@ -127,6 +140,7 @@ def generate_launch_description(): DeclareLaunchArgument("min_depth_m", default_value="0.15"), DeclareLaunchArgument("max_depth_m", default_value="2.0"), DeclareLaunchArgument("publish_camera_base_tf", default_value="false"), + DeclareLaunchArgument("publish_hand_eye_tf", default_value="true"), DeclareLaunchArgument("camera_base_parent_frame", default_value="base_link"), DeclareLaunchArgument("camera_base_child_frame", default_value="camera_color_optical_frame"), DeclareLaunchArgument("camera_base_tf_parent_frame", default_value=LaunchConfiguration("camera_base_parent_frame")), @@ -137,6 +151,13 @@ def generate_launch_description(): DeclareLaunchArgument("camera_base_tf_roll", default_value="0.0"), DeclareLaunchArgument("camera_base_tf_pitch", default_value="0.0"), DeclareLaunchArgument("camera_base_tf_yaw", default_value="0.0"), + DeclareLaunchArgument("hand_eye_matrix_path", default_value=""), + DeclareLaunchArgument("hand_eye_parent_frame", default_value="link_6"), + DeclareLaunchArgument("hand_eye_matrix_child_frame", default_value="camera_color_optical_frame"), + DeclareLaunchArgument("hand_eye_published_child_frame", default_value="camera_link"), + DeclareLaunchArgument("hand_eye_translation_scale", default_value="0.001"), + DeclareLaunchArgument("hand_eye_compose_with_existing_tf", default_value="true"), + DeclareLaunchArgument("hand_eye_compose_timeout_sec", default_value="5.0"), realsense_launch, rg2_trigger_launch, yolo_to_floor_place_launch, diff --git a/src/azas_bringup/launch/tumbler_floor_place.launch.py b/src/azas_bringup/launch/tumbler_floor_place.launch.py index 7fafc01..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.43, - 0.18, - 0.392, - 0.43, - 0.08, - 0.392, - 0.43, - -0.02, - 0.392, - 0.43, - -0.12, - 0.392, + 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/launch/workspace_collision_scene.launch.py b/src/azas_bringup/launch/workspace_collision_scene.launch.py new file mode 100644 index 0000000..4926932 --- /dev/null +++ b/src/azas_bringup/launch/workspace_collision_scene.launch.py @@ -0,0 +1,156 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +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(): + safety_config_path = LaunchConfiguration("safety_config_path") + publish_period_sec = LaunchConfiguration("publish_period_sec") + dispenser_collision_config_path = LaunchConfiguration( + "dispenser_collision_config_path" + ) + dispenser_collision_publish_period_sec = LaunchConfiguration( + "dispenser_collision_publish_period_sec" + ) + + workspace_collision_scene = Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + parameters=[ + { + "safety_config_path": ParameterValue( + safety_config_path, + value_type=str, + ), + "publish_period_sec": ParameterValue( + publish_period_sec, + value_type=float, + ), + "publish_collision_objects": ParameterValue( + LaunchConfiguration("publish_collision_objects"), + value_type=bool, + ), + "frame_id": ParameterValue(LaunchConfiguration("frame_id"), value_type=str), + "table_collision_enabled": ParameterValue( + LaunchConfiguration("table_collision_enabled"), + value_type=bool, + ), + "table_surface_z": ParameterValue( + LaunchConfiguration("table_surface_z"), + value_type=float, + ), + "table_thickness": ParameterValue( + LaunchConfiguration("table_thickness"), + value_type=float, + ), + "table_collision_expand_to_workspace_walls": ParameterValue( + LaunchConfiguration("table_collision_expand_to_workspace_walls"), + value_type=bool, + ), + "workspace_boundary_collision_enabled": ParameterValue( + LaunchConfiguration("workspace_boundary_collision_enabled"), + value_type=bool, + ), + "workspace_boundary_wall_thickness": ParameterValue( + LaunchConfiguration("workspace_boundary_wall_thickness"), + value_type=float, + ), + "workspace_boundary_wall_clearance": ParameterValue( + LaunchConfiguration("workspace_boundary_wall_clearance"), + value_type=float, + ), + } + ], + ) + + dispenser_collision_scene = Node( + package="azas_motion", + executable="measured_dispenser_collision_scene_node", + name="measured_dispenser_collision_scene_node", + output="screen", + condition=IfCondition(LaunchConfiguration("dispenser_collision_enabled")), + parameters=[ + { + "config_path": ParameterValue( + dispenser_collision_config_path, + value_type=str, + ), + "publish_period_sec": ParameterValue( + dispenser_collision_publish_period_sec, + value_type=float, + ), + "publish_collision_objects": ParameterValue( + LaunchConfiguration("dispenser_collision_publish_objects"), + value_type=bool, + ), + "publish_markers": ParameterValue( + LaunchConfiguration("dispenser_collision_publish_markers"), + value_type=bool, + ), + } + ], + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "config", "safety.yaml"] + ), + ), + DeclareLaunchArgument("publish_period_sec", default_value="2.0"), + DeclareLaunchArgument("publish_collision_objects", default_value="true"), + DeclareLaunchArgument("frame_id", default_value="base_link"), + DeclareLaunchArgument("table_collision_enabled", default_value="true"), + DeclareLaunchArgument("table_surface_z", default_value="0.0"), + DeclareLaunchArgument("table_thickness", default_value="0.04"), + DeclareLaunchArgument( + "table_collision_expand_to_workspace_walls", + default_value="true", + ), + DeclareLaunchArgument( + "workspace_boundary_collision_enabled", + default_value="true", + ), + DeclareLaunchArgument( + "workspace_boundary_wall_thickness", + default_value="0.04", + ), + DeclareLaunchArgument( + "workspace_boundary_wall_clearance", + default_value="0.02", + ), + DeclareLaunchArgument("dispenser_collision_enabled", default_value="true"), + DeclareLaunchArgument( + "dispenser_collision_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("azas_bringup"), + "config", + "measured_dispenser_collision.yaml", + ] + ), + ), + DeclareLaunchArgument( + "dispenser_collision_publish_period_sec", + default_value="2.0", + ), + DeclareLaunchArgument( + "dispenser_collision_publish_objects", + default_value="true", + ), + DeclareLaunchArgument( + "dispenser_collision_publish_markers", + default_value="true", + ), + workspace_collision_scene, + dispenser_collision_scene, + ] + ) diff --git a/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py b/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py index c132e63..4fbc2e1 100644 --- a/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py +++ b/src/azas_bringup/launch/yolo_cup_pick_node_legacy.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, Shutdown +from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -50,7 +51,7 @@ def generate_launch_description(): model_path_arg = DeclareLaunchArgument( "model_path", - default_value="/home/ssu/Azas/data/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + default_value="/home/ssu/Azas/local_models/best.pt", description="Path to trained cup YOLO weights.", ) conf_arg = DeclareLaunchArgument("conf", default_value="0.35") @@ -253,7 +254,7 @@ def generate_launch_description(): } ], ), - Node( + yolo_node := Node( package="azas_perception", executable="yolo_cup_pick_legacy_node", output="screen", @@ -375,5 +376,15 @@ def generate_launch_description(): }, ], ), + RegisterEventHandler( + OnProcessExit( + target_action=yolo_node, + on_exit=[ + Shutdown( + reason="yolo_cup_pick_legacy_node exited after pick attempt" + ) + ], + ) + ), ] ) 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..ea3afce 100644 --- a/src/azas_bringup/launch/yolo_to_floor_place.launch.py +++ b/src/azas_bringup/launch/yolo_to_floor_place.launch.py @@ -12,6 +12,7 @@ def generate_launch_description(): selected_dispenser_id = LaunchConfiguration("selected_dispenser_id") run_yolo = LaunchConfiguration("run_yolo") publish_camera_base_tf = LaunchConfiguration("publish_camera_base_tf") + publish_hand_eye_tf = LaunchConfiguration("publish_hand_eye_tf") camera_base_tf_x = LaunchConfiguration("camera_base_tf_x") camera_base_tf_y = LaunchConfiguration("camera_base_tf_y") camera_base_tf_z = LaunchConfiguration("camera_base_tf_z") @@ -93,10 +94,40 @@ def generate_launch_description(): ], condition=IfCondition(publish_camera_base_tf), ) + hand_eye_tf = Node( + package="azas_perception", + executable="hand_eye_static_tf_node", + name="hand_eye_static_tf_node", + output="screen", + condition=IfCondition(publish_hand_eye_tf), + parameters=[{ + "matrix_path": LaunchConfiguration("hand_eye_matrix_path"), + "parent_frame": LaunchConfiguration("hand_eye_parent_frame"), + "matrix_child_frame": LaunchConfiguration("hand_eye_matrix_child_frame"), + "published_child_frame": LaunchConfiguration("hand_eye_published_child_frame"), + "translation_scale": ParameterValue( + LaunchConfiguration("hand_eye_translation_scale"), + value_type=float, + ), + "compose_with_existing_tf": ParameterValue( + LaunchConfiguration("hand_eye_compose_with_existing_tf"), + value_type=bool, + ), + "compose_timeout_sec": ParameterValue( + LaunchConfiguration("hand_eye_compose_timeout_sec"), + value_type=float, + ), + }], + ) 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"), @@ -114,6 +145,7 @@ def generate_launch_description(): DeclareLaunchArgument("min_depth_m", default_value="0.15"), DeclareLaunchArgument("max_depth_m", default_value="2.0"), DeclareLaunchArgument("publish_camera_base_tf", default_value="false"), + DeclareLaunchArgument("publish_hand_eye_tf", default_value="true"), DeclareLaunchArgument("camera_base_parent_frame", default_value="base_link"), DeclareLaunchArgument("camera_base_child_frame", default_value="camera_color_optical_frame"), DeclareLaunchArgument("camera_base_tf_x", default_value="0.0"), @@ -122,6 +154,20 @@ def generate_launch_description(): DeclareLaunchArgument("camera_base_tf_roll", default_value="0.0"), DeclareLaunchArgument("camera_base_tf_pitch", default_value="0.0"), DeclareLaunchArgument("camera_base_tf_yaw", default_value="0.0"), + DeclareLaunchArgument( + "hand_eye_matrix_path", + default_value=PathJoinSubstitution([ + FindPackageShare("azas_perception"), + "config", + "T_gripper2camera.npy", + ]), + ), + DeclareLaunchArgument("hand_eye_parent_frame", default_value="link_6"), + DeclareLaunchArgument("hand_eye_matrix_child_frame", default_value="camera_color_optical_frame"), + DeclareLaunchArgument("hand_eye_published_child_frame", default_value="camera_link"), + DeclareLaunchArgument("hand_eye_translation_scale", default_value="0.001"), + DeclareLaunchArgument("hand_eye_compose_with_existing_tf", default_value="true"), + DeclareLaunchArgument("hand_eye_compose_timeout_sec", default_value="5.0"), DeclareLaunchArgument("device", default_value="cpu"), DeclareLaunchArgument("run_yolo", default_value="true"), DeclareLaunchArgument("enable_hardware", default_value="false"), @@ -138,6 +184,7 @@ def generate_launch_description(): DeclareLaunchArgument("tumbler_pose_wait_timeout", default_value="30.0"), yolo_launch, camera_base_tf, + hand_eye_tf, Node( package="azas_perception", executable="cup_detection_pose_bridge_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_bringup/rviz/azas_cocktail_collision_preview.rviz b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz new file mode 100644 index 0000000..a4e92d2 --- /dev/null +++ b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz @@ -0,0 +1,107 @@ +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: 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: + 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/collision_scene/markers + Name: Full Collision Scene + 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/azas_collision_only.rviz b/src/azas_bringup/rviz/azas_collision_only.rviz new file mode 100644 index 0000000..28a155d --- /dev/null +++ b/src/azas_bringup/rviz/azas_collision_only.rviz @@ -0,0 +1,72 @@ +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/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization_marker_array + Name: Collision Scene Default Topic + 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/collision_scene/markers + Name: Full Collision Scene + 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.15 + Focal Point: + X: 0.62 + Y: -0.02 + Z: 0.34 + Name: Current View + Pitch: 0.58 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.65 + 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/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..063ca22 100644 --- a/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz +++ b/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz @@ -28,38 +28,55 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Marker Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /azas/dispenser_sequence/markers - Name: Azas Sequence Markers - Value: false - - Class: rviz_default_plugins/Path - Buffer Length: 1 - Color: 204; 26; 255 - Enabled: false - Line Style: Lines - Line Width: 0.03 - Name: Azas Sequence Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.03 - Shaft Length: 0.1 - Shaft Radius: 0.01 - Topic: + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + 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/dispenser_sequence/plan - Value: false + 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/collision_scene/markers + Name: Full Collision Scene + 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: /visualization_marker_array + Name: Collision Scene Default Topic + 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_joint_preview/markers + Name: Measured Joint Preview Steps + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: 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/azas_rule_motion_preview.rviz b/src/azas_bringup/rviz/azas_rule_motion_preview.rviz new file mode 100644 index 0000000..c7e62fb --- /dev/null +++ b/src/azas_bringup/rviz/azas_rule_motion_preview.rviz @@ -0,0 +1,158 @@ +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 + TF Prefix: "" + 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/collision_scene/markers + Name: Full Collision Scene + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + 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: false + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/dispenser_sequence/markers + Name: Rule Motion Cup And Steps + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/measured_joint_preview/markers + Name: Measured Joint Preview Steps + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /jarvis/robot_arm/markers + Name: Rule Motion Demo Arm + 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 + Value: true + - Class: rviz_default_plugins/Path + Buffer Length: 1 + Color: 255; 230; 0 + Enabled: true + Line Style: Lines + Line Width: 0.025 + Name: Rule Motion Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 230; 0 + Pose Style: None + Radius: 0.03 + Shaft Length: 0.1 + Shaft Radius: 0.01 + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/dispenser_sequence/plan + 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.25 + Focal Point: + X: 0.48 + Y: -0.16 + Z: 0.30 + Name: Current View + Pitch: 0.62 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.55 + 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/color_scan_pose.rviz b/src/azas_bringup/rviz/color_scan_pose.rviz new file mode 100644 index 0000000..03b0ec0 --- /dev/null +++ b/src/azas_bringup/rviz/color_scan_pose.rviz @@ -0,0 +1,99 @@ +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: 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: + 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_bringup/rviz/link6_gripper_tcp_debug.rviz b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz new file mode 100644 index 0000000..6a05ca7 --- /dev/null +++ b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz @@ -0,0 +1,146 @@ +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: /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: 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 + 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 + - 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: 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..4bfbc1b 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, @@ -22,7 +23,9 @@ license="MIT", entry_points={ "console_scripts": [ + "collision_scene_rviz_publisher = azas_bringup.collision_scene_rviz_publisher:main", "joint_state_relay_legacy = azas_bringup.joint_state_relay_legacy:main", + "rule_motion_joint_preview_node = azas_bringup.rule_motion_joint_preview_node:main", ], }, ) 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..b2513e2 --- /dev/null +++ b/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 0000000..f7e6a18 Binary files /dev/null and b/src/azas_cup_uprighting/azas_cup_uprighting/T_gripper2camera.npy differ 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..4e93d86 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -0,0 +1,520 @@ +"""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 os +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 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 + + +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 그리퍼 통합 베이스.""" + + 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.color_image = None + self.depth_image = None + self.intrinsics = None + + # ── 픽 상태 ── + self.declare_parameter("auto_pick", False) + self.declare_parameter("exit_after_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} + self._auto_mode = _parse_bool(self.get_parameter("auto_pick").value) + self._exit_after_pick = _parse_bool(self.get_parameter("exit_after_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 + + # ── 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 ── + # 현장 실행은 카메라 확인이 먼저다. 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) + 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 모델 로드 완료") + + # ── 카메라 구독 ── + 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 _ensure_moveit(self) -> bool: + if self.robot is not None: + return True + + log = self.get_logger() + log.info("MoveItPy 지연 초기화 중...") + 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.08, acc=0.05, time=3.0) + self.pilz_params = self._make_plan_params( + "pilz_industrial_motion_planner", "PTP", vel=0.06, acc=0.04, time=3.0) + self.on_moveit_ready() + log.info("MoveItPy 지연 초기화 완료") + return True + + # ── 콜백 ── + 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._imgmsg_to_bgr(msg) + + def _depth_cb(self, msg): + 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 래퍼 + # ════════════════════════════════════════════ + 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, + 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: + 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, + 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, + 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() + 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 + if not self._ensure_moveit(): + return None + 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(): + success = False + try: + success = bool(self.detect_and_pick(frame)) + finally: + self._frozen_frame = None + if success and self._exit_after_pick: + self.get_logger().info("exit_after_pick=true and pick completed; closing cup_uprighting node") + rclpy.shutdown() + + 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 on_moveit_ready(self): + """MoveIt 지연 초기화 직후 호출. 자식이 planning scene 설정 가능.""" + 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() + if self._skip_initial_home_move: + 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 + 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() + # exit_after_pick 경로 등에서 컨텍스트가 이미 닫혀 있으면 재호출 시 + # RuntimeError로 exit code 1이 되어 라우터가 실패로 오인한다. + if rclpy.ok(): + 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..e18b4a0 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py @@ -0,0 +1,101 @@ + +import math +import os +import yaml +from ament_index_python.packages import PackageNotFoundError, 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 ──────────────────────────────────────────── +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] + +# ── 카메라 토픽 ────────────────────────────────────── +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..c0b35bf --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py @@ -0,0 +1,189 @@ +"""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 + + + +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 _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, + 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() + + 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 + + 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) + return bool(result) + + +def get_gripper_pose_by_cup(cup_theta): + """ + 컵의 주축 각도(theta)를 받아 그리퍼가 옆면(허리)을 수직 진입하여 + 파지할 수 있도록 쿼터니언 반환 + """ + yaw = -cup_theta + + # 오일러 각을 쿼터니언으로 변환 + 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..1d1a64b --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py @@ -0,0 +1,230 @@ +"""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_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"] + + 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 _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: + 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 + + # Bounding Box 좌표를 정수로 변환하여 ROI(관심 영역) 자르기 + x1, y1, x2, y2 = map(int, bbox) + roi = frame[y1:y2, x1:x2] + + if roi.size == 0: + return 0.0 + + # 이미지를 흑백으로 변환하고 이진화(Threshold)하여 컵과 배경 분리 + gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) + _, thresh = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) + + # 외곽선(Contours) 찾기 + contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + return 0.0 + + # 가장 넓은 외곽선을 컵의 본체로 간주 + c = max(contours, key=cv2.contourArea) + + # ── PCA: 컵 외곽선 점들의 주축 방향 ── + pts = c.reshape(-1, 2).astype(np.float64) + mean = pts.mean(axis=0) + pts_centered = pts - mean + cov = np.cov(pts_centered.T) + eigvals, eigvecs = np.linalg.eigh(cov) + principal = eigvecs[:, np.argmax(eigvals)] # 가장 큰 분산 방향 = 컵 장축 + + theta = np.arctan2(principal[1], principal[0]) + 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 + + # 컵 중심에서 스티커(무게중심)를 향하는 벡터 생성 + vec_sticker = np.array([cm_x - center_x, cm_y - center_y]) + + # theta 각도가 가리키는 단위 벡터 생성 + vec_theta = np.array([np.cos(theta), np.sin(theta)]) + + # 두 벡터의 내적(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..4379a59 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python3 +""" +쓰러진 컵을 인식하고 카메라 방향을 컵 입구에 맞춰 파지한 뒤 홈 위치로 복귀하는 노드. +""" + +import time +import numpy as np +import cv2 + +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__() + + def on_moveit_ready(self): + 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 _draw_detections(self, frame: np.ndarray) -> np.ndarray: + vis = super()._draw_detections(frame) + + for det in self._detections: + if det["cls_name"] != "cup": + continue + + x1, y1, x2, y2 = det["box"] + cx, cy = det["cx"], det["cy"] + + # 컵 주축 각도 계산 + theta = calculate_cup_orientation(self.depth_image, det["box"], frame) + + # 입구 방향 판별 + is_top = is_top_pointing_towards_theta(frame, det["box"], theta) + + top_theta = theta if is_top else theta + np.pi + + length = max(x2 - x1, y2 - y1) // 2 + dx = int(np.cos(theta) * length) + dy = int(np.sin(theta) * length) + cv2.line(vis, (cx - dx, cy - dy), (cx + dx, cy + dy), (255, 255, 255), 2) + + # 입구 방향 화살표 (초록), 바닥 방향 화살표 (파랑) + top_dx = int(np.cos(top_theta) * length) + top_dy = int(np.sin(top_theta) * length) + bot_dx = int(np.cos(top_theta + np.pi) * length) + bot_dy = int(np.sin(top_theta + np.pi) * length) + + cv2.arrowedLine(vis, (cx, cy), (cx + top_dx, cy + top_dy), + (0, 255, 0), 3, tipLength=0.3) # 초록 = 입구 + cv2.arrowedLine(vis, (cx, cy), (cx + bot_dx, cy + bot_dy), + (255, 100, 0), 2, tipLength=0.2) # 파랑 = 바닥 + + label = f"top={'YES' if is_top else 'NO'} theta={np.degrees(theta):.1f}deg" + cv2.putText(vis, label, (x1, y2 + 18), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + + return vis + + 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 + else: + log.info("[VISION] 컵이 정방향입니다. 기본 파지 방향을 유지합니다.") + + self.picking = True + try: + # feature 브랜치의 핵심 목표인 홈 복귀 시퀀스 직접 호출 + 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): + 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 + + PICK_CLEARANCE = 0.02 + + pick_z = floor_z + CUP_RADIUS_M + Z_OFFSET + PICK_CLEARANCE + safe_z = floor_z + 0.25 + Z_OFFSET + + log.info(f"== [FEATURE] 컵 구출 및 홈 위치 복귀 시퀀스 가동 (각도: {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() + 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, + } + + # 1-1 단계 실패 시 예외 처리 및 탈출 + if not self.plan_pose(bx, by, safe_z, current_ori): + log.error("[1-1] 상공 진입 실패. 시퀀스 중단.") + 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 False + time.sleep(1.0) + + log.info("[2] 컵 파지 위치 하강") + if not self.plan_pose(bx, by, pick_z, target_ori): + log.error("[2] 파지 위치 하강 실패. 시퀀스 중단.") + return False + + self.gripper.close_gripper() + log.info("[2] 그리퍼 클로즈 완료") + time.sleep(1.0) + + log.info("[3] 리프트업 (안전 고도로 재상승)") + if not self.plan_pose(bx, by, safe_z, target_ori): + log.error("[3] 리프트업 실패. 물체 탈락 위험으로 인한 안전 복구 가동.") + self.gripper.open_gripper() + log.info("=> 그리퍼 비상 강제 릴리즈 완료.") + 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): + 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..0b9f488 --- /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: "/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" + 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.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: 0.08 + max_acceleration_scaling_factor: 0.05 + 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: 0.08 + max_acceleration_scaling_factor: 0.05 + 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.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: 0.08 + max_acceleration_scaling_factor: 0.05 + planning_time: 1.5 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..a5d73d1 --- /dev/null +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -0,0 +1,189 @@ +from copy import deepcopy + +from launch import LaunchDescription +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 +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 + + +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"), + "exit_after_pick": LaunchConfiguration("exit_after_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, + }, + ], + # exit_after_pick으로 메인 노드가 끝나면 collision/tf 보조 노드들도 함께 + # 정리해 launch가 종료되도록 한다. 안 그러면 라우터가 영원히 대기한다. + on_exit=Shutdown(), + ) + return [yolo_cup_uprighting_node] + + +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"] + ) + 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.", + ) + exit_after_pick_arg = DeclareLaunchArgument( + "exit_after_pick", + default_value="false", + description="Exit the OpenCV node after one successful manual/auto upright sequence.", + ) + 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.", + ) + 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( + 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(), + ) + + 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, + }], + ) + + return LaunchDescription([ + model_path_arg, + publish_hand_eye_tf_arg, + auto_pick_arg, + exit_after_pick_arg, + skip_initial_home_move_arg, + moveit_controller_name_arg, + controller_action_name_arg, + workspace_collision_scene, + world_base_tf, + hand_eye_tf, + OpaqueFunction( + function=_runtime_nodes, + args=[moveit_config.to_dict(), moveit_py_params], + ), + ]) diff --git a/src/azas_cup_uprighting/package.xml b/src/azas_cup_uprighting/package.xml new file mode 100644 index 0000000..a7efc79 --- /dev/null +++ b/src/azas_cup_uprighting/package.xml @@ -0,0 +1,34 @@ + + + + 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 + azas_perception + + 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', + ], + }, +) diff --git a/src/azas_description/CMakeLists.txt b/src/azas_description/CMakeLists.txt new file mode 100644 index 0000000..7c0ce23 --- /dev/null +++ b/src/azas_description/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.8) +project(azas_description) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch meshes urdf + DESTINATION share/${PROJECT_NAME} +) + +install( + DIRECTORY LICENSES + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/azas_description/LICENSES/OnRobot-RG2FT-ROS-MIT.txt b/src/azas_description/LICENSES/OnRobot-RG2FT-ROS-MIT.txt new file mode 100644 index 0000000..169256c --- /dev/null +++ b/src/azas_description/LICENSES/OnRobot-RG2FT-ROS-MIT.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Ian Chuang + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/src/azas_description/launch/view_m0609_rg2_parametric.launch.py b/src/azas_description/launch/view_m0609_rg2_parametric.launch.py new file mode 100644 index 0000000..3382337 --- /dev/null +++ b/src/azas_description/launch/view_m0609_rg2_parametric.launch.py @@ -0,0 +1,54 @@ +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(): + model_path = PathJoinSubstitution( + [ + FindPackageShare("azas_description"), + "urdf", + "m0609_rg2_parametric.urdf.xacro", + ] + ) + + robot_description = ParameterValue( + Command( + [ + FindExecutable(name="xacro"), + " ", + model_path, + " rg2_mount_rpy:=", + LaunchConfiguration("rg2_mount_rpy"), + ] + ), + value_type=str, + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "rg2_mount_rpy", + default_value="0 0 0", + ), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[{"robot_description": robot_description}], + output="screen", + ), + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + output="screen", + ), + Node( + package="rviz2", + executable="rviz2", + output="screen", + ), + ] + ) diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/angle_bracket.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/angle_bracket.stl new file mode 100644 index 0000000..780c9af Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/angle_bracket.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/gripper_body.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/gripper_body.stl new file mode 100644 index 0000000..f2bd0cc Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/gripper_body.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_finger.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_finger.stl new file mode 100644 index 0000000..1669843 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_finger.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_knuckle.stl new file mode 100644 index 0000000..f0f7913 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/left_inner_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/left_outer_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/left_outer_knuckle.stl new file mode 100644 index 0000000..9bd321d Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/left_outer_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/quick_changer.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/quick_changer.stl new file mode 100644 index 0000000..9320f92 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/quick_changer.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_finger.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_finger.stl new file mode 100644 index 0000000..1a5f2a5 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_finger.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_knuckle.stl new file mode 100644 index 0000000..1b3d2fa Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/right_inner_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/collision/right_outer_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/collision/right_outer_knuckle.stl new file mode 100644 index 0000000..58dfddf Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/collision/right_outer_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/angle_bracket.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/angle_bracket.stl new file mode 100644 index 0000000..0335d1c Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/angle_bracket.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/gripper_body.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/gripper_body.stl new file mode 100644 index 0000000..b923461 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/gripper_body.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_finger.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_finger.stl new file mode 100644 index 0000000..a9b24d0 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_finger.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_knuckle.stl new file mode 100644 index 0000000..e5c73df Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/left_inner_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/left_outer_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/left_outer_knuckle.stl new file mode 100644 index 0000000..64d0851 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/left_outer_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/quick_changer.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/quick_changer.stl new file mode 100644 index 0000000..288ca20 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/quick_changer.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_finger.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_finger.stl new file mode 100644 index 0000000..73edc68 Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_finger.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_knuckle.stl new file mode 100644 index 0000000..4b12cae Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/right_inner_knuckle.stl differ diff --git a/src/azas_description/meshes/onrobot_rg2ft/visual/right_outer_knuckle.stl b/src/azas_description/meshes/onrobot_rg2ft/visual/right_outer_knuckle.stl new file mode 100644 index 0000000..88b442e Binary files /dev/null and b/src/azas_description/meshes/onrobot_rg2ft/visual/right_outer_knuckle.stl differ diff --git a/src/azas_description/package.xml b/src/azas_description/package.xml new file mode 100644 index 0000000..e6d5c1b --- /dev/null +++ b/src/azas_description/package.xml @@ -0,0 +1,16 @@ + + + azas_description + 0.1.0 + Azas robot description overlays, including the parametric OnRobot RG2 gripper model. + Azas + Apache-2.0 + MIT + + ament_cmake + xacro + + + ament_cmake + + diff --git a/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro new file mode 100644 index 0000000..56eaf1e --- /dev/null +++ b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + diff --git a/src/azas_description/urdf/onrobot_rg2ft_mesh_macro.xacro b/src/azas_description/urdf/onrobot_rg2ft_mesh_macro.xacro new file mode 100644 index 0000000..40811da --- /dev/null +++ b/src/azas_description/urdf/onrobot_rg2ft_mesh_macro.xacro @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/azas_description/urdf/rg2_parametric.xacro b/src/azas_description/urdf/rg2_parametric.xacro new file mode 100644 index 0000000..04d02ec --- /dev/null +++ b/src/azas_description/urdf/rg2_parametric.xacro @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 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/azas_dispenser/dispenser_press_node.py b/src/azas_dispenser/azas_dispenser/dispenser_press_node.py index ee002f3..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,18 +951,17 @@ 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): - 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 @@ -886,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 @@ -915,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_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", diff --git a/src/azas_gripper/azas_gripper/rg2_gripper_node.py b/src/azas_gripper/azas_gripper/rg2_gripper_node.py index 112ba4c..9b2c9a0 100644 --- a/src/azas_gripper/azas_gripper/rg2_gripper_node.py +++ b/src/azas_gripper/azas_gripper/rg2_gripper_node.py @@ -1,8 +1,8 @@ import rclpy from azas_interfaces.srv import SetGripper +from std_srvs.srv import Trigger from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from std_srvs.srv import Trigger class RG2GripperNode(Node): @@ -29,28 +29,21 @@ def __init__(self): self.gripper = self._connect_real_gripper() self.create_service(SetGripper, "/azas/gripper/open_close", self.on_set_gripper) - self.create_service( - Trigger, - self.get_parameter("open_service").get_parameter_value().string_value, - self.on_open, - ) - self.create_service( - Trigger, - self.get_parameter("close_service").get_parameter_value().string_value, - self.on_close, - ) - self.create_service( - SetGripper, - self.get_parameter("set_width_service").get_parameter_value().string_value, - self.on_set_gripper, + open_service = self.get_parameter("open_service").get_parameter_value().string_value + close_service = self.get_parameter("close_service").get_parameter_value().string_value + set_width_service = self.get_parameter("set_width_service").get_parameter_value().string_value + self.create_service(SetGripper, set_width_service, self.on_set_gripper) + self.create_service(Trigger, open_service, self.on_open_trigger) + self.create_service(Trigger, close_service, self.on_close_trigger) + service_text = ( + f"/azas/gripper/open_close plus compatibility aliases " + f"{open_service}, {close_service}, {set_width_service}" ) if self.use_real_hardware: - self.get_logger().info( - "RG2 hardware services ready on /azas/gripper/open_close and /jarvis/rg2/*" - ) + self.get_logger().info(f"RG2 hardware services ready on {service_text}") else: self.get_logger().warn( - "Dry-run gripper services ready on /azas/gripper/open_close and /jarvis/rg2/*; " + f"Dry-run gripper services ready on {service_text}; " "set use_real_hardware:=true to command the real RG2" ) @@ -71,15 +64,17 @@ def _force_n_to_register_units(self, force_n): force_units = int(round(force_n * 10.0)) return max(0, min(force_units, self.gripper.max_force)) - def on_set_gripper(self, request, response): - command = request.command.lower() - if command not in {"open", "close", "set_width", "preopen", "grasp"}: - response.success = False - response.message = f"unsupported command: {request.command}" - return response - - width_m = float(request.width_m) - force_n = float(request.force_n) + def _dispatch_command(self, command, width_m, force_n): + command = command.lower().strip() + if command == "preopen": + command = "open" + elif command == "grasp": + command = "set_width" + if command not in {"open", "close", "set_width"}: + return False, f"unsupported command: {command}" + + width_m = float(width_m) + force_n = float(force_n) if force_n <= 0.0: force_n = ( self.get_parameter("default_force_n").get_parameter_value().double_value @@ -106,46 +101,38 @@ def on_set_gripper(self, request, response): force_units = self._force_n_to_register_units(force_n) self.gripper.move_gripper(width_units, force_units) except Exception as exc: - response.success = False - response.message = f"RG2 command failed: {exc}" - self.get_logger().error(response.message) - return response + message = f"RG2 command failed: {exc}" + self.get_logger().error(message) + return False, message - response.success = True - response.message = ( + return True, ( f"sent RG2 {command} command " f"width_units={width_units} force_units={force_units}" ) - return response - response.success = True - response.message = ( - "accepted dry-run command; real RG2 was not commanded" - ) - return response + return True, "accepted dry-run command; real RG2 was not commanded" - def on_open(self, _request, response): - req = SetGripper.Request() - req.command = "open" - req.width_m = self.get_parameter("default_open_width_m").get_parameter_value().double_value - req.force_n = self.get_parameter("default_force_n").get_parameter_value().double_value - set_response = SetGripper.Response() - self.on_set_gripper(req, set_response) - response.success = bool(set_response.success) - response.message = str(set_response.message) + def on_open_trigger(self, _request, response): + success, message = self._dispatch_command("open", 0.0, 0.0) + response.success = success + response.message = message return response - def on_close(self, _request, response): - req = SetGripper.Request() - req.command = "close" - req.width_m = self.get_parameter("default_close_width_m").get_parameter_value().double_value - req.force_n = self.get_parameter("default_force_n").get_parameter_value().double_value - set_response = SetGripper.Response() - self.on_set_gripper(req, set_response) - response.success = bool(set_response.success) - response.message = str(set_response.message) + def on_close_trigger(self, _request, response): + success, message = self._dispatch_command("close", 0.0, 0.0) + response.success = success + response.message = message return response + def on_set_gripper(self, request, response): + success, message = self._dispatch_command( + request.command, + request.width_m, + request.force_n, + ) + response.success = success + response.message = message + return response def main(args=None): rclpy.init(args=args) 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..15405d8 --- /dev/null +++ b/src/azas_kiosk/azas_kiosk/kiosk_node.py @@ -0,0 +1,291 @@ +from __future__ import annotations + +import json +import mimetypes +import random +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 + +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): + 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") + + self._lock = threading.Lock() + self._state: dict[str, Any] = { + "started_at": time.time(), + "last_command": "", + "last_confirmation": "", + "selected_recipe_id": "", + "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("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("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, + *, + selected_recipe_id: str = "", + local_confirmation: str = "", + clear_selection: bool = False, + ) -> 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() + 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) + 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_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) + 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() + 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": result["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 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]) -> 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 { + "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/confirm": + return {"text": "응", "local_confirmation": "제조 시작 요청을 보냈습니다."} + if path == "/api/cancel": + return { + "text": "취소", + "local_confirmation": "주문을 취소했습니다.", + "clear_selection": True, + } + if path == "/api/command": + 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: + 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..0626b3b --- /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..6cd16d3 --- /dev/null +++ b/src/azas_kiosk/launch/azas_kiosk.launch.py @@ -0,0 +1,32 @@ +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"), + DeclareLaunchArgument( + "cocktail_status_topic", default_value="/azas/cocktail/status" + ), + 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"), + "cocktail_status_topic": LaunchConfiguration("cocktail_status_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..de0dc7d --- /dev/null +++ b/src/azas_kiosk/test/test_menu_catalog.py @@ -0,0 +1,35 @@ +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(): + menus = build_menu_payload() + + assert [menu["recipe_id"] for menu in menus] == [ + "recipe_01", + "recipe_02", + "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 new file mode 100644 index 0000000..f75dcf0 --- /dev/null +++ b/src/azas_kiosk/web/app.js @@ -0,0 +1,141 @@ +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: "Berry", + yellow: "Citrus", + green: "Herb", + blue: "Ocean", +}; + +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) { + state.selectedRecipeId = recipeId || null; + 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 || "주문을 기다리고 있습니다."; + + if (payload.selected_recipe_id) { + setSelectedCard(payload.selected_recipe_id); + } + 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); + const result = await postJson("/api/recommend"); + if (result.recipe_id) { + setSelectedCard(result.recipe_id); + } + 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..3e7a82e --- /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..3741a1a --- /dev/null +++ b/src/azas_kiosk/web/styles.css @@ -0,0 +1,436 @@ +: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"); +} + +body, +button { + -webkit-tap-highlight-color: transparent; +} + +button { + border: 0; + font: inherit; +} + +.shell { + display: grid; + grid-template-rows: auto auto; + gap: 14px; + width: min(100%, 540px); + min-height: 100vh; + margin: 0 auto; + padding: 14px; +} + +.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: 14px; + padding: 18px; +} + +.topbar { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 18px; +} + +.eyebrow, +.panel-label { + margin: 0 0 6px; + color: #617067; + font-size: 12px; + font-weight: 700; + letter-spacing: 0; + text-transform: uppercase; +} + +h1 { + margin: 0; + font-size: 36px; + line-height: 1.08; + 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: 1fr; + gap: 12px; + flex: 1; +} + +.menu-card { + display: grid; + grid-template-columns: minmax(0, 1fr) 108px; + grid-template-rows: auto minmax(0, 1fr); + gap: 12px; + min-height: 176px; + padding: 18px; + 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: 30px; + line-height: 1.08; + letter-spacing: 0; + overflow-wrap: anywhere; +} + +.menu-tone { + grid-column: 1 / 2; + 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-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: 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: 1fr 1fr 1.35fr; + gap: 10px; + position: sticky; + bottom: 12px; + z-index: 2; + padding-top: 4px; +} + +.primary-button, +.secondary-button { + min-height: 64px; + border-radius: 8px; + font-size: 20px; + 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; + gap: 14px; + padding: 16px; +} + +.large-status { + margin: 0; + font-size: 22px; + font-weight: 900; + line-height: 1.24; + letter-spacing: 0; +} + +.status-list { + display: grid; + grid-template-columns: 1fr 1fr; + gap: 10px; +} + +.status-list div { + display: grid; + gap: 6px; + padding: 12px; + 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: 15px; + letter-spacing: 0; +} + +@media (min-width: 760px) { + .shell { + width: min(100%, 680px); + padding: 24px; + } + + 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 { + 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_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..ecd3de0 --- /dev/null +++ b/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py @@ -0,0 +1,472 @@ +#!/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 + +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} +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 + moveit_execution_settle_sec: float + press_only: bool + + +@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_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: + 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.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), + ) + + +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}") + status = str(block.get("press_contact_status", "")).strip() + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + raise ValueError( + f"dispenser_outlets.{cfg.dispenser_id}.press_contact_joints_deg is marked " + f"{status!r}; refusing dispenser press cycle until PRESS{cfg.dispenser_id}_CONTACT is re-taught" + ) + 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}") + 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) + + +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}") + 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) + + +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}") + 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) + + +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_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" + 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") + + # 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: + 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 61a0746..e257600 100644 --- a/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py +++ b/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py @@ -11,9 +11,11 @@ import math from dataclasses import dataclass +from pathlib import Path as FsPath from typing import List, Sequence, Tuple import rclpy +import yaml from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 from nav_msgs.msg import Path from rclpy.node import Node @@ -29,6 +31,7 @@ class SequenceStep: label: str xyz: XYZ + cup_base_xyz: XYZ | None = None def point(xyz: XYZ) -> Point: @@ -60,15 +63,34 @@ def __init__(self) -> None: super().__init__("dispenser_sequence_preview_node") self.declare_parameter("cup_pose_topic", "/azas/sim/tumbler_pose") self.declare_parameter("frame_id", "base_link") + self.declare_parameter( + "calibration_path", + "/home/ssu/Azas/src/azas_bringup/config/calibration.yaml", + ) + self.declare_parameter( + "dispenser_collision_config_path", + "/home/ssu/Azas/src/azas_bringup/config/measured_dispenser_collision.yaml", + ) self.declare_parameter("selected_dispenser_id", 2) self.declare_parameter("cup_height_m", 0.17) self.declare_parameter("grasp_height_m", 0.085) 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("move_release_offset_x_m", 0.0) + self.declare_parameter("move_release_offset_y_m", 0.0) + self.declare_parameter("move_release_offset_z_m", 0.0) + self.declare_parameter("press_pre_lift_retreat_x_m", -0.050) + self.declare_parameter("press_pre_lift_retreat_y_m", 0.0) + self.declare_parameter("regrasp_retreat_x_m", -0.080) + self.declare_parameter("regrasp_retreat_y_m", 0.0) + self.declare_parameter("regrasp_rear_entry_offset_x_m", -0.080) + self.declare_parameter("regrasp_rear_entry_offset_y_m", 0.0) + self.declare_parameter("regrasp_min_transit_z_m", 0.500) + self.declare_parameter("regrasp_max_transit_z_m", 0.560) self.declare_parameter("publish_rate_hz", 4.0) self.declare_parameter("show_sequence_markers", True) self.declare_parameter("show_dispenser_markers", True) @@ -95,20 +117,25 @@ def __init__(self) -> None: self.declare_parameter( "dispenser_outlet_positions", [ - 0.43, - 0.18, - 0.392, - 0.43, - 0.08, - 0.392, - 0.43, - -0.02, - 0.392, - 0.43, - -0.12, - 0.392, + 0.555, + -0.100, + 0.093, + 0.549, + -0.150, + 0.097, + 0.527, + -0.204, + 0.107, + 0.517, + -0.235, + 0.109, ], ) + self.calibration = self.load_yaml_config("calibration_path", "measured dispenser sequence calibration") + self.dispenser_collision = self.load_yaml_config( + "dispenser_collision_config_path", + "measured dispenser front-hold collision config", + ) plan_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) self.path_pub = self.create_publisher(Path, "/azas/dispenser_sequence/plan", plan_qos) @@ -136,6 +163,23 @@ def __init__(self) -> None: "Azas RViz dispenser sequence preview ready; hardware and camera execution are disabled." ) + def load_yaml_config(self, parameter_name: str, label: str) -> dict: + config_path = FsPath(str(self.get_parameter(parameter_name).value)).expanduser() + if not config_path.exists(): + self.get_logger().warning( + f"{parameter_name} does not exist; using launch fallback where available: {config_path}" + ) + return {} + with config_path.open("r", encoding="utf-8") as stream: + config = yaml.safe_load(stream) or {} + if not isinstance(config, dict): + self.get_logger().warning( + f"{parameter_name} is not a YAML map; using launch fallback where available: {config_path}" + ) + return {} + self.get_logger().info(f"Loaded {label}: {config_path}") + return config + def on_cup_pose(self, msg: PoseStamped) -> None: expected = str(self.get_parameter("frame_id").value) if msg.header.frame_id != expected: @@ -149,7 +193,9 @@ def on_cup_pose(self, msg: PoseStamped) -> None: def dispenser_layout(self) -> tuple[List[XYZ], List[XYZ], int]: bottles = triples(self.get_parameter("dispenser_bottle_positions").value) - outlets = triples(self.get_parameter("dispenser_outlet_positions").value) + outlets = self.measured_points("outlet_pose_xyz_m") + if not outlets: + outlets = triples(self.get_parameter("dispenser_outlet_positions").value) if len(bottles) != len(outlets): raise ValueError("bottle/outlet count mismatch") selected = min( @@ -158,24 +204,121 @@ def dispenser_layout(self) -> tuple[List[XYZ], List[XYZ], int]: ) return bottles, outlets, selected - 1 + def measured_points(self, field_name: str) -> List[XYZ]: + outlets = self.calibration.get("dispenser_outlets", {}) + if not isinstance(outlets, dict): + return [] + points: List[XYZ] = [] + for dispenser_id in sorted(outlets.keys(), key=lambda value: int(value)): + config = outlets.get(dispenser_id) or {} + if not isinstance(config, dict): + return [] + xyz = config.get(field_name) + if not isinstance(xyz, list) or len(xyz) != 3: + return [] + points.append((float(xyz[0]), float(xyz[1]), float(xyz[2]))) + return points + + def measured_front_holds(self) -> List[XYZ]: + front_holds = self.dispenser_collision.get("front_hold_poses", {}) + if not isinstance(front_holds, dict): + return [] + points: List[XYZ] = [] + for dispenser_id in sorted( + front_holds.keys(), + key=lambda value: int(str(value).replace("dispenser_", "")), + ): + config = front_holds.get(dispenser_id) or {} + if not isinstance(config, dict): + return [] + xyz = config.get("position_xyz_m") + if not isinstance(xyz, list) or len(xyz) != 3: + return [] + points.append((float(xyz[0]), float(xyz[1]), float(xyz[2]))) + return points + def build_steps(self, cup_pose: PoseStamped) -> List[SequenceStep]: _, outlets, selected_index = self.dispenser_layout() + press_targets = self.measured_points("press_pose_xyz_m") + front_holds = self.measured_front_holds() cup = cup_pose.pose.position cup_base = (float(cup.x), float(cup.y), float(cup.z)) outlet = outlets[selected_index] + measured_front_hold = front_holds[selected_index] if len(front_holds) == len(outlets) else outlet grasp_height = float(self.get_parameter("grasp_height_m").value) side_offset = float(self.get_parameter("side_pre_grasp_offset_m").value) lift_height = float(self.get_parameter("lift_height_m").value) shake_clearance = float(self.get_parameter("shake_clearance_m").value) shake_swing = float(self.get_parameter("shake_swing_m").value) shake_lift = float(self.get_parameter("shake_lift_m").value) + release_dx = float(self.get_parameter("move_release_offset_x_m").value) + release_dy = float(self.get_parameter("move_release_offset_y_m").value) + release_dz = float(self.get_parameter("move_release_offset_z_m").value) + press_retreat_x = float(self.get_parameter("press_pre_lift_retreat_x_m").value) + press_retreat_y = float(self.get_parameter("press_pre_lift_retreat_y_m").value) + regrasp_retreat_x = float(self.get_parameter("regrasp_retreat_x_m").value) + regrasp_retreat_y = float(self.get_parameter("regrasp_retreat_y_m").value) + rear_entry_x = float(self.get_parameter("regrasp_rear_entry_offset_x_m").value) + rear_entry_y = float(self.get_parameter("regrasp_rear_entry_offset_y_m").value) + regrasp_min_z = float(self.get_parameter("regrasp_min_transit_z_m").value) + regrasp_max_z = float(self.get_parameter("regrasp_max_transit_z_m").value) grasp = (cup_base[0], cup_base[1], cup_base[2] + grasp_height) side_pre_grasp = (grasp[0], grasp[1] - side_offset, grasp[2]) low_transfer_z = grasp[2] + lift_height 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) + front_lane = (measured_front_hold[0] - 0.12, grasp[1], low_transfer_z) + outlet_front_hold = ( + measured_front_hold[0] + release_dx, + measured_front_hold[1] + release_dy, + measured_front_hold[2] + release_dz, + ) + cup_front_base = ( + outlet_front_hold[0], + outlet_front_hold[1], + outlet_front_hold[2] - grasp_height, + ) + press_retreat = ( + outlet_front_hold[0] + press_retreat_x, + outlet_front_hold[1] + press_retreat_y, + outlet_front_hold[2], + ) + empty_lift = ( + press_retreat[0], + press_retreat[1], + max(regrasp_min_z, outlet_front_hold[2] + 0.20), + ) + if len(press_targets) == len(outlets): + press_down = press_targets[selected_index] + else: + press_down = ( + outlet[0] - 0.03, + outlet[1], + max(outlet[2] + 0.035, low_transfer_z + 0.12), + ) + press_ready = ( + press_down[0], + press_down[1], + min(max(press_down[2] + 0.12, 0.50), 0.56), + ) + regrasp_high_z = min(max(regrasp_min_z, outlet_front_hold[2] + 0.25), regrasp_max_z) + post_press_lift = ( + press_down[0], + press_down[1], + regrasp_high_z, + ) + regrasp_high = ( + outlet_front_hold[0] + regrasp_retreat_x, + outlet_front_hold[1] + regrasp_retreat_y, + regrasp_high_z, + ) + regrasp_rear_low = ( + outlet_front_hold[0] + rear_entry_x, + outlet_front_hold[1] + rear_entry_y, + 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 +347,23 @@ 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 retreat_back_before_press_lift", press_retreat, cup_front_base), + SequenceStep("8 lift_empty_gripper", empty_lift, cup_front_base), + SequenceStep("9 press_ready", press_ready, cup_front_base), + SequenceStep("10 press_dispenser", press_down, cup_front_base), + SequenceStep("11 lift_after_press_open_gripper", post_press_lift, cup_front_base), + SequenceStep("12 retreat_back_high_after_press", regrasp_high, cup_front_base), + SequenceStep("13 lower_at_rear_entry", regrasp_rear_low, cup_front_base), + SequenceStep("14 forward_regrasp_cup", regrasp, cup_front_base), + SequenceStep("15 regrasp_lift", regrasp_lift), + SequenceStep("16 shake_center", shake_center), + SequenceStep("17 shake_left", shake_left), + SequenceStep("18 shake_right", shake_right), + SequenceStep("19 shake_forward", shake_forward), + SequenceStep("20 shake_back", shake_back), + SequenceStep("21 shake_recenter", shake_center), ] def publish_preview(self) -> None: @@ -341,7 +494,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 +508,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 +527,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 +574,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 +582,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 +590,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 +606,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 +624,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 94108b5..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.60, - 0.08, - 0.392, - 0.60, - 0.02, - 0.392, - 0.60, - -0.04, - 0.392, - 0.60, - -0.10, - 0.392, + 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/lid_grip.py b/src/azas_motion/azas_motion/lid_grip.py new file mode 100644 index 0000000..78181bb --- /dev/null +++ b/src/azas_motion/azas_motion/lid_grip.py @@ -0,0 +1,180 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass + +import numpy as np +from geometry_msgs.msg import Pose + + +@dataclass(frozen=True) +class LidGripConfig: + approach_offset_m: float = 0.08 + lift_offset_m: float = 0.10 + min_approach_z_m: float = 0.0 + surface_offset_m: float = 0.0 + offset_axis: str = "local_z" + tcp_grasp_offset_x_m: float = 0.0 + tcp_grasp_offset_y_m: float = 0.0 + tcp_grasp_offset_z_m: float = 0.0 + min_grasp_z_m: float = 0.02 + max_grasp_z_m: float = 0.60 + + +@dataclass(frozen=True) +class LidGripPlan: + approach_pose: Pose + grasp_pose: Pose + lift_pose: Pose + approach_offset_m: float + lift_offset_m: float + + +def compute_lid_grip_plan(lid_pose: Pose, config: LidGripConfig) -> LidGripPlan: + """Compute plan-only lid approach/grasp/lift candidates from a detected pose. + + The lid pose must already come from live perception and TF conversion. This + function does not know robot coordinates, collision state, or gripper width. + """ + _validate_config(config) + _validate_pose(lid_pose) + + grasp_pose = _translated_by_base_offset( + lid_pose, + np.array( + [ + config.tcp_grasp_offset_x_m, + config.tcp_grasp_offset_y_m, + config.tcp_grasp_offset_z_m, + ], + dtype=float, + ), + ) + grasp_pose = _translated_along_offset_axis( + grasp_pose, + config.surface_offset_m, + config.offset_axis, + ) + if grasp_pose.position.z < config.min_grasp_z_m or grasp_pose.position.z > config.max_grasp_z_m: + raise ValueError( + "LID_GRASP_Z_OUT_OF_BOUNDS: " + f"grasp_z={grasp_pose.position.z:.3f} " + f"outside [{config.min_grasp_z_m:.3f}, {config.max_grasp_z_m:.3f}]" + ) + + approach_pose = _translated_along_offset_axis( + grasp_pose, + config.approach_offset_m, + config.offset_axis, + ) + if approach_pose.position.z < config.min_approach_z_m: + approach_pose.position.z = config.min_approach_z_m + lift_pose = _translated_along_offset_axis( + grasp_pose, + config.lift_offset_m, + config.offset_axis, + ) + return LidGripPlan( + approach_pose=approach_pose, + grasp_pose=grasp_pose, + lift_pose=lift_pose, + approach_offset_m=config.approach_offset_m, + lift_offset_m=config.lift_offset_m, + ) + + +def _validate_config(config: LidGripConfig) -> None: + values = ( + config.approach_offset_m, + config.lift_offset_m, + config.min_approach_z_m, + config.surface_offset_m, + config.tcp_grasp_offset_x_m, + config.tcp_grasp_offset_y_m, + config.tcp_grasp_offset_z_m, + config.min_grasp_z_m, + config.max_grasp_z_m, + ) + if not all(math.isfinite(value) for value in values): + raise ValueError("lid grip config values must be finite") + if config.approach_offset_m <= 0.0: + raise ValueError("approach_offset_m must be positive") + if config.lift_offset_m <= 0.0: + raise ValueError("lift_offset_m must be positive") + if config.min_approach_z_m < 0.0: + raise ValueError("min_approach_z_m must be non-negative") + if config.min_grasp_z_m > config.max_grasp_z_m: + raise ValueError("min_grasp_z_m must be <= max_grasp_z_m") + if config.offset_axis not in ("local_z", "base_z"): + raise ValueError("offset_axis must be 'local_z' or 'base_z'") + + +def _validate_pose(pose: Pose) -> None: + values = ( + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) + if not all(math.isfinite(value) for value in values): + raise ValueError("lid pose values must be finite") + norm = math.sqrt( + pose.orientation.x * pose.orientation.x + + pose.orientation.y * pose.orientation.y + + pose.orientation.z * pose.orientation.z + + pose.orientation.w * pose.orientation.w + ) + if norm <= 1e-12: + raise ValueError("lid pose quaternion norm must be non-zero") + + +def _translated_along_offset_axis(pose: Pose, distance_m: float, offset_axis: str) -> Pose: + if offset_axis == "base_z": + z_axis = np.array([0.0, 0.0, 1.0], dtype=float) + else: + z_axis = _local_z_axis(pose) + output = _copy_pose(pose) + output.position.x += float(distance_m) * z_axis[0] + output.position.y += float(distance_m) * z_axis[1] + output.position.z += float(distance_m) * z_axis[2] + return output + + +def _translated_by_base_offset(pose: Pose, offset: np.ndarray) -> Pose: + output = _copy_pose(pose) + output.position.x += float(offset[0]) + output.position.y += float(offset[1]) + output.position.z += float(offset[2]) + return output + + +def _local_z_axis(pose: Pose) -> np.ndarray: + qx = float(pose.orientation.x) + qy = float(pose.orientation.y) + qz = float(pose.orientation.z) + qw = float(pose.orientation.w) + norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) + qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm + return np.array( + [ + 2.0 * (qx * qz + qy * qw), + 2.0 * (qy * qz - qx * qw), + 1.0 - 2.0 * (qx * qx + qy * qy), + ], + dtype=float, + ) + + +def _copy_pose(source: Pose) -> Pose: + pose = Pose() + pose.position.x = source.position.x + pose.position.y = source.position.y + pose.position.z = source.position.z + pose.orientation.x = source.orientation.x + pose.orientation.y = source.orientation.y + pose.orientation.z = source.orientation.z + pose.orientation.w = source.orientation.w + return pose diff --git a/src/azas_motion/azas_motion/lid_grip_planner_node.py b/src/azas_motion/azas_motion/lid_grip_planner_node.py new file mode 100644 index 0000000..6fa34fe --- /dev/null +++ b/src/azas_motion/azas_motion/lid_grip_planner_node.py @@ -0,0 +1,3611 @@ +from __future__ import annotations + +import copy +import json +import math +import threading +import time + +import rclpy +from azas_interfaces.srv import SetGripper +from azas_motion.lid_grip import LidGripConfig, compute_lid_grip_plan +from geometry_msgs.msg import PoseStamped +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.node import Node +from std_msgs.msg import String + +try: + from dsr_msgs2.srv import ( + GetCurrentPosj, + GetCurrentPosx, + GetLastAlarm, + Ikin, + MoveJoint, + MoveLine, + MovePeriodic, + ReleaseComplianceCtrl, + ReleaseForce, + SetDesiredForce, + TaskComplianceCtrl, + ) +except ImportError: # pragma: no cover - depends on the robot workspace overlay + GetCurrentPosj = None + GetCurrentPosx = None + GetLastAlarm = None + Ikin = None + MoveJoint = None + MoveLine = None + MovePeriodic = None + ReleaseComplianceCtrl = None + ReleaseForce = None + SetDesiredForce = None + TaskComplianceCtrl = None + + +DR_BASE = 0 +DR_TOOL = 1 +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" + + +def service_name(prefix: str, name: str) -> str: + clean_prefix = prefix.strip("/") + clean_name = name.strip("/") + if not clean_prefix: + return f"/{clean_name}" + return f"/{clean_prefix}/{clean_name}" + + +class LidGripPlannerNode(Node): + """Supervised lid grip pose generator. + + The node consumes a base_link lid pose from perception and publishes + approach/grasp/lift PoseStamped candidates. Doosan motion and RG2 service + calls are disabled by default and require explicit hardware gate + parameters plus a p-key trigger from the preview node. + """ + + def __init__(self): + super().__init__("lid_grip_planner_node") + self.declare_parameter("lid_pose_topic", "/jarvis/lid_gripper/lid_pose") + self.declare_parameter("trigger_topic", "/jarvis/lid_gripper/grip_request") + self.declare_parameter("approach_pose_topic", "/jarvis/lid_gripper/approach_pose") + self.declare_parameter("grasp_pose_topic", "/jarvis/lid_gripper/grasp_pose") + self.declare_parameter("lift_pose_topic", "/jarvis/lid_gripper/lift_pose") + self.declare_parameter("status_topic", "/jarvis/lid_gripper/status") + self.declare_parameter("require_base_link_pose", True) + self.declare_parameter("plan_on_pose", True) + self.declare_parameter("log_pose_plans", False) + self.declare_parameter("log_korean_status", True) + self.declare_parameter("log_json_status", False) + self.declare_parameter("approach_offset_m", 0.08) + self.declare_parameter("lift_offset_m", 0.10) + self.declare_parameter("min_approach_z_m", 0.0) + self.declare_parameter("surface_offset_m", 0.0) + self.declare_parameter("offset_axis", "local_z") + self.declare_parameter("tcp_grasp_offset_x_m", 0.0) + self.declare_parameter("tcp_grasp_offset_y_m", 0.0) + self.declare_parameter("tcp_grasp_offset_z_m", 0.0) + self.declare_parameter("min_grasp_z_m", 0.02) + self.declare_parameter("max_grasp_z_m", 0.60) + self.declare_parameter("enable_hardware", False) + self.declare_parameter("hardware_confirm", "") + self.declare_parameter("allow_service_control_without_moveit", False) + self.declare_parameter("service_prefix", "") + self.declare_parameter("rx", 180.0) + self.declare_parameter("ry", 0.0) + self.declare_parameter("rz", 180.0) + self.declare_parameter("use_lid_pose_yaw_for_pick", False) + self.declare_parameter( + "lid_pose_yaw_axis", + "x", + ParameterDescriptor(dynamic_typing=True), + ) + 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) + 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) + self.declare_parameter("verify_motion_reached", True) + self.declare_parameter("motion_verify_timeout_sec", 20.0) + self.declare_parameter("motion_target_tolerance_m", 0.015) + self.declare_parameter("visual_refine_before_grasp", False) + self.declare_parameter("visual_refine_sample_count", 10) + self.declare_parameter("visual_refine_timeout_sec", 2.0) + self.declare_parameter("visual_refine_min_sample_interval_sec", 0.03) + self.declare_parameter("visual_refine_max_yaw_std_deg", 3.0) + self.declare_parameter("visual_refine_max_position_std_m", 0.005) + self.declare_parameter("visual_refine_apply_xy", True) + self.declare_parameter("visual_refine_apply_yaw", True) + self.declare_parameter("visual_refine_fallback_to_initial_plan", True) + self.declare_parameter("settle_seconds_before_grasp", 0.5) + self.declare_parameter("hold_seconds_after_grasp", 0.2) + self.declare_parameter("enable_lid_twist_after_grasp", False) + self.declare_parameter("lid_twist_target_x_m", math.nan) + self.declare_parameter("lid_twist_target_y_m", math.nan) + self.declare_parameter("lid_twist_target_z_m", math.nan) + self.declare_parameter("lid_twist_rx", math.nan) + self.declare_parameter("lid_twist_ry", math.nan) + self.declare_parameter("lid_twist_rz", math.nan) + self.declare_parameter("lid_twist_use_force_control", False) + self.declare_parameter("lid_twist_use_force_spiral", False) + self.declare_parameter("lid_twist_press_down_m", 0.0) + self.declare_parameter("lid_twist_rz_delta_deg", -30.0) + self.declare_parameter("lid_twist_turn_step_deg", 90.0) + self.declare_parameter("lid_twist_transfer_clearance_m", 0.0) + self.declare_parameter("lid_twist_release_lift_m", 0.03) + self.declare_parameter("lid_twist_min_z_m", 0.02) + self.declare_parameter("lid_twist_max_z_m", 0.60) + self.declare_parameter("lid_twist_transfer_max_z_m", 0.60) + self.declare_parameter("lid_twist_transfer_velocity", 30.0) + self.declare_parameter("lid_twist_press_velocity", 8.0) + self.declare_parameter("lid_twist_turn_velocity", 8.0) + self.declare_parameter("lid_twist_acceleration", 10.0) + self.declare_parameter("lid_twist_hold_seconds_before_turn", 0.3) + self.declare_parameter("lid_twist_hold_seconds_after_turn", 0.5) + self.declare_parameter("lid_twist_down_force_n", 8.0) + self.declare_parameter("lid_twist_force_ref", "base") + self.declare_parameter("lid_twist_force_rotation_mode", "movel") + self.declare_parameter("lid_twist_force_service_timeout_sec", 5.0) + self.declare_parameter("lid_twist_regrip_cycles", 1) + self.declare_parameter("lid_twist_regrip_turn_deg", math.nan) + self.declare_parameter("lid_twist_regrip_reset_between_cycles", False) + 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) + self.declare_parameter("lid_twist_preseat_periodic_z_amp_mm", 1.0) + self.declare_parameter("lid_twist_preseat_periodic_rx_amp_deg", 0.0) + self.declare_parameter("lid_twist_preseat_periodic_ry_amp_deg", 3.0) + self.declare_parameter("lid_twist_preseat_periodic_rz_amp_deg", 5.0) + self.declare_parameter("lid_twist_preseat_periodic_period_sec", 1.0) + 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) + self.declare_parameter("lid_twist_compliance_rx_stiffness", 200.0) + self.declare_parameter("lid_twist_compliance_ry_stiffness", 200.0) + self.declare_parameter("lid_twist_compliance_rz_stiffness", 200.0) + self.declare_parameter("motion_target_orientation_tolerance_deg", 3.0) + self.declare_parameter("enable_gripper_service_calls", False) + self.declare_parameter("execute_gripper_on_pose", False) + self.declare_parameter("gripper_set_service", "/jarvis/rg2/set_width") + self.declare_parameter("gripper_preopen_width_m", math.nan) + self.declare_parameter("gripper_grasp_width_m", math.nan) + self.declare_parameter("gripper_force_n", math.nan) + self.declare_parameter("gripper_wait_timeout_sec", 2.0) + self.declare_parameter("continue_after_gripper_grasp_failure", False) + self.declare_parameter("gripper_grasp_failure_wait_sec", 2.0) + + self._latest_pose: PoseStamped | None = None + self._latest_pose_lock = threading.Lock() + self._last_plan_stamp = 0.0 + self._sequence_lock = threading.Lock() + self._approach_pub = self.create_publisher( + PoseStamped, + str(self.get_parameter("approach_pose_topic").value), + 10, + ) + self._grasp_pub = self.create_publisher( + PoseStamped, + str(self.get_parameter("grasp_pose_topic").value), + 10, + ) + self._lift_pub = self.create_publisher( + PoseStamped, + str(self.get_parameter("lift_pose_topic").value), + 10, + ) + self._status_pub = self.create_publisher( + String, + str(self.get_parameter("status_topic").value), + 10, + ) + self._service_prefix = str(self.get_parameter("service_prefix").value) + self._hardware_armed = self._is_hardware_armed() + self._move_joint_client = None + self._move_line_client = None + self._move_periodic_client = None + self._ikin_client = None + self._current_posj_client = None + self._current_posx_client = None + self._last_alarm_client = None + self._task_compliance_client = None + self._set_desired_force_client = None + self._release_force_client = None + self._release_compliance_client = None + self._move_joint_service_name = service_name(self._service_prefix, "motion/move_joint") + self._move_line_service_name = service_name(self._service_prefix, "motion/move_line") + self._move_periodic_service_name = service_name(self._service_prefix, "motion/move_periodic") + self._ikin_service_name = service_name(self._service_prefix, "motion/ikin") + self._current_posj_service_name = service_name( + self._service_prefix, + "aux_control/get_current_posj", + ) + self._current_posx_service_name = service_name( + self._service_prefix, + "aux_control/get_current_posx", + ) + self._last_alarm_service_name = service_name( + self._service_prefix, + "system/get_last_alarm", + ) + self._task_compliance_service_name = service_name( + self._service_prefix, + "force/task_compliance_ctrl", + ) + self._set_desired_force_service_name = service_name( + self._service_prefix, + "force/set_desired_force", + ) + self._release_force_service_name = service_name( + self._service_prefix, + "force/release_force", + ) + self._release_compliance_service_name = service_name( + self._service_prefix, + "force/release_compliance_ctrl", + ) + if self._hardware_armed: + if MoveLine is None: + self.get_logger().error( + "dsr_msgs2 is unavailable; lid MoveLine execution is disabled" + ) + self._hardware_armed = False + else: + if MoveJoint is not None: + self._move_joint_client = self.create_client( + MoveJoint, + self._move_joint_service_name, + ) + self._move_line_client = self.create_client( + MoveLine, + self._move_line_service_name, + ) + if MovePeriodic is not None: + self._move_periodic_client = self.create_client( + MovePeriodic, + self._move_periodic_service_name, + ) + if Ikin is not None: + self._ikin_client = self.create_client(Ikin, self._ikin_service_name) + if GetCurrentPosj is not None: + self._current_posj_client = self.create_client( + GetCurrentPosj, + self._current_posj_service_name, + ) + if GetCurrentPosx is not None: + self._current_posx_client = self.create_client( + GetCurrentPosx, + self._current_posx_service_name, + ) + if GetLastAlarm is not None: + self._last_alarm_client = self.create_client( + GetLastAlarm, + self._last_alarm_service_name, + ) + if TaskComplianceCtrl is not None: + self._task_compliance_client = self.create_client( + TaskComplianceCtrl, + self._task_compliance_service_name, + ) + if SetDesiredForce is not None: + self._set_desired_force_client = self.create_client( + SetDesiredForce, + self._set_desired_force_service_name, + ) + if ReleaseForce is not None: + self._release_force_client = self.create_client( + ReleaseForce, + self._release_force_service_name, + ) + if ReleaseComplianceCtrl is not None: + self._release_compliance_client = self.create_client( + ReleaseComplianceCtrl, + self._release_compliance_service_name, + ) + self._gripper_client = self.create_client( + SetGripper, + str(self.get_parameter("gripper_set_service").value), + ) + self.create_subscription( + PoseStamped, + str(self.get_parameter("lid_pose_topic").value), + self._on_lid_pose, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("trigger_topic").value), + self._on_grip_request, + 10, + ) + self.get_logger().warn( + "[뚜껑픽] 노드 준비 완료: p 입력이 들어오면 hardware gate 확인 후 " + "approach -> grasp -> lift 순서로 실행합니다. " + "enable_lid_twist_after_grasp=true이면 lift 이후 teach point에서 뚜껑 회전을 이어서 실행합니다." + ) + + def _on_lid_pose(self, msg: PoseStamped) -> None: + with self._latest_pose_lock: + self._latest_pose = copy.deepcopy(msg) + if self._sequence_lock.locked(): + return + if not bool(self.get_parameter("plan_on_pose").value): + return + self._plan_from_pose( + msg, + request_source="pose", + allow_gripper=bool(self.get_parameter("execute_gripper_on_pose").value), + allow_motion=False, + ) + + def _on_grip_request(self, msg: String) -> None: + try: + 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", + request=payload, + real_motion=False, + ) + return + pose_snapshot = self._latest_pose_snapshot() + if pose_snapshot is None: + self._publish_status( + "failed", + error="NO_BASE_LINK_LID_POSE_FOR_TRIGGER", + request=payload, + real_motion=False, + ) + return + + if not self._sequence_lock.acquire(blocking=False): + self._publish_status( + "trigger_ignored_motion_in_progress", + request=payload, + real_motion=self._hardware_armed, + ) + return + + 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, request_source), + daemon=True, + ).start() + + @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=request_source, + allow_gripper=bool(self.get_parameter("enable_gripper_service_calls").value), + allow_motion=True, + ) + except Exception as exc: # pragma: no cover - final safety net for hardware path + self._publish_status( + "failed", + error=f"trigger sequence exception: {exc}", + real_motion=self._hardware_armed, + ) + finally: + self._sequence_lock.release() + + def _plan_from_pose( + self, + msg: PoseStamped, + request_source: str, + allow_gripper: bool, + allow_motion: bool, + ) -> None: + if bool(self.get_parameter("require_base_link_pose").value) and msg.header.frame_id != "base_link": + self._publish_status( + "failed", + error="LID_POSE_NOT_BASE_LINK", + request_source=request_source, + frame_id=msg.header.frame_id, + real_motion=False, + ) + return + + try: + plan = compute_lid_grip_plan(msg.pose, self._config()) + except ValueError as exc: + self._publish_status( + "failed", + error=str(exc), + request_source=request_source, + real_motion=False, + ) + return + + self._approach_pub.publish(self._pose_stamped(plan.approach_pose, msg)) + self._grasp_pub.publish(self._pose_stamped(plan.grasp_pose, msg)) + self._lift_pub.publish(self._pose_stamped(plan.lift_pose, msg)) + self._last_plan_stamp = time.monotonic() + self._publish_status( + "planned", + request_source=request_source, + frame_id=msg.header.frame_id, + approach_offset_m=plan.approach_offset_m, + lift_offset_m=plan.lift_offset_m, + approach=self._pose_xyz(plan.approach_pose), + grasp=self._pose_xyz(plan.grasp_pose), + lift=self._pose_xyz(plan.lift_pose), + motion_allowed=bool(allow_motion), + hardware_armed=self._hardware_armed, + real_motion=False, + **self._pick_rz_selection(plan.grasp_pose)[1], + ) + + if allow_motion: + self._try_motion_sequence(msg, plan, allow_gripper=allow_gripper) + elif allow_gripper: + self._try_gripper_sequence() + + def _latest_pose_snapshot(self) -> PoseStamped | None: + with self._latest_pose_lock: + if self._latest_pose is None: + return None + return copy.deepcopy(self._latest_pose) + + def _is_hardware_armed(self) -> bool: + return all( + ( + bool(self.get_parameter("enable_hardware").value), + str(self.get_parameter("hardware_confirm").value) == HARDWARE_CONFIRM_PHRASE, + bool(self.get_parameter("allow_service_control_without_moveit").value), + ) + ) + + def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: bool) -> None: + if not bool(self.get_parameter("enable_hardware").value): + self._publish_status( + "trigger_planned_no_motion", + note="enable_hardware is false; p key produced plan only", + real_motion=False, + ) + return + if not self._hardware_armed: + self._publish_status( + "failed", + error=( + "hardware gates incomplete; require enable_hardware:=true, " + f"hardware_confirm:={HARDWARE_CONFIRM_PHRASE}, and " + "allow_service_control_without_moveit:=true" + ), + real_motion=False, + ) + return + if self._move_line_client is None: + self._publish_status( + "failed", + error="MoveLine client unavailable", + 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( + "failed", + error=f"MoveLine service unavailable: {self._move_line_service_name}", + 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( + "failed", + error="GetCurrentPosx client unavailable; refusing motion without target verification", + real_motion=False, + ) + return + if not self._current_posx_client.wait_for_service(timeout_sec=max(timeout_sec, 0.0)): + self._publish_status( + "failed", + error=f"GetCurrentPosx service unavailable: {self._current_posx_service_name}", + real_motion=False, + ) + return + gripper_targets = None + if allow_gripper: + gripper_targets = self._gripper_targets() + if gripper_targets is None: + return + if not self._gripper_client.wait_for_service( + timeout_sec=max(float(self.get_parameter("gripper_wait_timeout_sec").value), 0.0) + ): + self._publish_status( + "failed", + error=f"gripper service unavailable: {self.get_parameter('gripper_set_service').value}", + real_motion=False, + ) + 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), + ("grasp_lid", plan.grasp_pose), + ("lift_lid", plan.lift_pose), + ) + if not self._precheck_ikin_steps(initial_steps if use_visual_refine else full_steps): + return + 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 + + if use_visual_refine: + 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: + return + if not self._precheck_ikin_steps(refined_steps): + return + motion_steps = refined_steps + else: + motion_steps = motion_steps_without_refine + + for label, target in motion_steps: + 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): + return + sequence_steps = [label for label, _target in motion_steps] + if use_visual_refine: + sequence_steps.insert(0, "approach_lid") + 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 requests were sent through the configured service", + ) + + def _finish_grasp_at_current_pose(self, gripper_targets) -> bool: + settle_seconds = max(float(self.get_parameter("settle_seconds_before_grasp").value), 0.0) + if settle_seconds > 0.0: + self._publish_status( + "settling_before_grasp", + seconds=round(settle_seconds, 3), + real_motion=True, + ) + time.sleep(settle_seconds) + if gripper_targets is not None: + _preopen_width, grasp_width, force_n = gripper_targets + if not self._call_gripper_sync("grasp", grasp_width, force_n, publish_failure=False): + if not bool(self.get_parameter("continue_after_gripper_grasp_failure").value): + self._publish_status( + "failed", + error="RG2 grasp request timed out or returned success=false", + real_motion=True, + ) + return False + fallback_wait = max( + float(self.get_parameter("gripper_grasp_failure_wait_sec").value), + 0.0, + ) + self._publish_status( + "gripper_grasp_continue_after_failure", + seconds=round(fallback_wait, 3), + real_motion=True, + ) + time.sleep(fallback_wait) + 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) + min_interval_sec = max( + float(self.get_parameter("visual_refine_min_sample_interval_sec").value), + 0.0, + ) + 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, + timeout_sec=round(timeout_sec, 3), + axis=axis, + real_motion=True, + ) + samples = self._collect_visual_refine_samples( + sample_count=sample_count, + timeout_sec=timeout_sec, + min_interval_sec=min_interval_sec, + axis=axis, + ) + if len(samples) < sample_count: + return self._visual_refine_fallback_steps( + fallback_plan, + reason="not_enough_samples", + details={ + "samples": len(samples), + "required_samples": sample_count, + }, + ) + + stats = self._visual_refine_stats(samples, axis=axis) + max_position_std_m = max( + float(self.get_parameter("visual_refine_max_position_std_m").value), + 0.0, + ) + max_yaw_std_deg = max( + float(self.get_parameter("visual_refine_max_yaw_std_deg").value), + 0.0, + ) + apply_xy = bool(self.get_parameter("visual_refine_apply_xy").value) + apply_yaw = bool(self.get_parameter("visual_refine_apply_yaw").value) + if apply_xy and stats["position_std_m"] > max_position_std_m: + return self._visual_refine_fallback_steps( + fallback_plan, + reason="position_unstable", + details={ + "samples": len(samples), + "position_std_m": round(stats["position_std_m"], 5), + "max_position_std_m": round(max_position_std_m, 5), + }, + ) + if apply_yaw and stats["yaw_std_deg"] > max_yaw_std_deg: + return self._visual_refine_fallback_steps( + fallback_plan, + reason="yaw_unstable", + details={ + "samples": len(samples), + "yaw_std_deg": round(stats["yaw_std_deg"], 3), + "max_yaw_std_deg": round(max_yaw_std_deg, 3), + }, + ) + + refined_msg = copy.deepcopy(source_msg) + if apply_xy: + refined_msg.pose.position.x = stats["mean_x"] + refined_msg.pose.position.y = stats["mean_y"] + if apply_yaw: + refined_msg.pose.orientation = copy.deepcopy(samples[-1][0].pose.orientation) + try: + refined_plan = compute_lid_grip_plan(refined_msg.pose, self._config()) + except ValueError as exc: + return self._visual_refine_fallback_steps( + fallback_plan, + reason="plan_failed", + details={"error": str(exc)}, + ) + + 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) + self._approach_pub.publish(self._pose_stamped(refined_plan.approach_pose, refined_msg)) + self._grasp_pub.publish(self._pose_stamped(refined_plan.grasp_pose, refined_msg)) + self._lift_pub.publish(self._pose_stamped(refined_plan.lift_pose, refined_msg)) + self._publish_status( + "visual_refine_result", + samples=len(samples), + position_std_m=round(stats["position_std_m"], 5), + yaw_std_deg=round(stats["yaw_std_deg"], 3), + mean_x=round(stats["mean_x"], 4), + mean_y=round(stats["mean_y"], 4), + mean_yaw_deg=round(stats["mean_yaw_deg"], 3), + pick_rz_deg=round(refined_rz, 3), + axis=axis, + mean_yaw_x_deg=self._round_or_none(stats.get("mean_yaw_x_deg"), 3), + mean_yaw_y_deg=self._round_or_none(stats.get("mean_yaw_y_deg"), 3), + pick_rz_x_deg=self._round_or_none(stats.get("pick_rz_x_deg"), 3), + pick_rz_y_deg=self._round_or_none(stats.get("pick_rz_y_deg"), 3), + apply_xy=apply_xy, + apply_yaw=apply_yaw, + real_motion=True, + ) + return ( + ("visual_refine_align_lid", refined_plan.approach_pose), + ("grasp_lid", refined_plan.grasp_pose), + ("lift_lid", refined_plan.lift_pose), + ) + + def _visual_refine_fallback_steps(self, fallback_plan, *, reason: str, details: dict): + if not bool(self.get_parameter("visual_refine_fallback_to_initial_plan").value): + detail = f"VISUAL_REFINE_FAILED_WITHOUT_FALLBACK: {reason}" + if details: + detail += f" {details}" + self._publish_status( + "failed", + error=detail, + real_motion=False, + ) + return None + self._publish_status( + "visual_refine_fallback", + reason=reason, + **details, + real_motion=True, + ) + return ( + ("grasp_lid", fallback_plan.grasp_pose), + ("lift_lid", fallback_plan.lift_pose), + ) + + def _collect_visual_refine_samples( + self, + *, + sample_count: int, + timeout_sec: float, + min_interval_sec: float, + axis: str, + ) -> list[tuple[PoseStamped, float]]: + start_stamp_key = self._pose_stamp_key(self._latest_pose_snapshot()) + seen_stamp_keys = set() + samples: list[tuple[PoseStamped, float]] = [] + deadline = time.monotonic() + timeout_sec + while rclpy.ok() and time.monotonic() < deadline and len(samples) < sample_count: + snapshot = self._latest_pose_snapshot() + if snapshot is None: + time.sleep(max(min_interval_sec, 0.01)) + continue + stamp_key = self._pose_stamp_key(snapshot) + if stamp_key is None or stamp_key == start_stamp_key or stamp_key in seen_stamp_keys: + time.sleep(max(min_interval_sec, 0.01)) + continue + if ( + bool(self.get_parameter("require_base_link_pose").value) + and snapshot.header.frame_id != "base_link" + ): + seen_stamp_keys.add(stamp_key) + time.sleep(max(min_interval_sec, 0.01)) + continue + yaw = self._pose_axis_yaw_deg(snapshot.pose, axis) + if yaw is None: + seen_stamp_keys.add(stamp_key) + time.sleep(max(min_interval_sec, 0.01)) + continue + samples.append((snapshot, yaw)) + seen_stamp_keys.add(stamp_key) + time.sleep(max(min_interval_sec, 0.01)) + return samples + + @staticmethod + def _pose_stamp_key(msg: PoseStamped | None) -> tuple[int, int] | None: + if msg is None: + return None + return (int(msg.header.stamp.sec), int(msg.header.stamp.nanosec)) + + def _visual_refine_stats(self, samples: list[tuple[PoseStamped, float]], *, axis: str) -> dict: + xs = [float(sample.pose.position.x) for sample, _yaw in samples] + ys = [float(sample.pose.position.y) for sample, _yaw in samples] + yaws = [float(yaw) for _sample, yaw in samples] + mean_x = sum(xs) / len(xs) + mean_y = sum(ys) / len(ys) + position_std_m = math.sqrt( + sum((x - mean_x) ** 2 + (y - mean_y) ** 2 for x, y in zip(xs, ys)) / len(xs) + ) + 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( + self._equivalent_angle_delta_deg(yaw, mean_yaw, period_deg=period_deg) ** 2 + for yaw in yaws + ) + / len(yaws) + ) + fixed_rz = float(self.get_parameter("rz").value) + requested_rz = mean_yaw + offset_deg + pick_rz = self._nearest_equivalent_angle_deg( + requested_rz, + reference_deg=fixed_rz, + period_deg=period_deg, + ) + x_stats = self._axis_yaw_stats(samples, "x", period_deg, offset_deg, fixed_rz) + y_stats = self._axis_yaw_stats(samples, "y", period_deg, offset_deg, fixed_rz) + return { + "mean_x": mean_x, + "mean_y": mean_y, + "position_std_m": position_std_m, + "axis": axis, + "mean_yaw_deg": mean_yaw, + "yaw_std_deg": yaw_std, + "pick_rz_deg": pick_rz, + "mean_yaw_x_deg": x_stats["mean_yaw_deg"], + "yaw_x_std_deg": x_stats["yaw_std_deg"], + "pick_rz_x_deg": x_stats["pick_rz_deg"], + "mean_yaw_y_deg": y_stats["mean_yaw_deg"], + "yaw_y_std_deg": y_stats["yaw_std_deg"], + "pick_rz_y_deg": y_stats["pick_rz_deg"], + } + + def _axis_yaw_stats( + self, + samples: list[tuple[PoseStamped, float]], + axis: str, + period_deg: float, + offset_deg: float, + fixed_rz: float, + ) -> dict: + yaws = [] + for sample, _selected_yaw in samples: + yaw = self._pose_axis_yaw_deg(sample.pose, axis) + if yaw is not None: + yaws.append(float(yaw)) + if not yaws: + return { + "mean_yaw_deg": None, + "yaw_std_deg": None, + "pick_rz_deg": None, + } + mean_yaw = self._mean_equivalent_angle_deg(yaws, period_deg=period_deg) + yaw_std = math.sqrt( + sum( + self._equivalent_angle_delta_deg(yaw, mean_yaw, period_deg=period_deg) ** 2 + for yaw in yaws + ) + / len(yaws) + ) + pick_rz = self._nearest_equivalent_angle_deg( + mean_yaw + offset_deg, + reference_deg=fixed_rz, + period_deg=period_deg, + ) + return { + "mean_yaw_deg": mean_yaw, + "yaw_std_deg": yaw_std, + "pick_rz_deg": pick_rz, + } + + @staticmethod + def _mean_equivalent_angle_deg(angles_deg: list[float], *, period_deg: float) -> float: + if not angles_deg: + return math.nan + if not (math.isfinite(period_deg) and abs(period_deg) > 1e-9): + period_deg = 360.0 + scale = 360.0 / abs(period_deg) + sin_sum = 0.0 + cos_sum = 0.0 + for angle in angles_deg: + radians = math.radians(float(angle) * scale) + sin_sum += math.sin(radians) + cos_sum += math.cos(radians) + if abs(sin_sum) <= 1e-12 and abs(cos_sum) <= 1e-12: + return float(angles_deg[-1]) + return math.degrees(math.atan2(sin_sum, cos_sum)) / scale + + @staticmethod + def _equivalent_angle_delta_deg(angle_deg: float, reference_deg: float, *, period_deg: float) -> float: + if not (math.isfinite(period_deg) and abs(period_deg) > 1e-9): + period_deg = 360.0 + period = abs(period_deg) + return (float(angle_deg) - float(reference_deg) + period / 2.0) % period - period / 2.0 + + def _call_movel(self, label: str, pose) -> bool: + if isinstance(pose, (list, tuple)): + pos = [float(value) for value in pose] + else: + pos = self._pose_to_dsr_pos(pose) + return self._call_movel_pos( + label, + pos, + velocity=float(self.get_parameter("line_velocity").value), + acceleration=float(self.get_parameter("line_acceleration").value), + 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, + pos_mm_deg: list[float], + *, + velocity: float, + acceleration: float, + verify_orientation: bool, + ) -> bool: + req = MoveLine.Request() + req.pos = [float(value) for value in pos_mm_deg] + req.vel = [float(velocity)] * 2 + req.acc = [float(acceleration)] * 2 + 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 + self.get_logger().warn( + f"[뚜껑픽] 이동 시작: {self._step_ko(label)} " + f"target=({req.pos[0] / 1000.0:.3f}, {req.pos[1] / 1000.0:.3f}, " + f"{req.pos[2] / 1000.0:.3f}) m rpy=({req.pos[3]:.1f}, {req.pos[4]:.1f}, {req.pos[5]:.1f}) deg" + ) + future = self._move_line_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} timed out or failed", + real_motion=True, + ) + return False + if not bool(result.success): + self._publish_status( + "failed", + error=f"{label} returned success=false", + real_motion=True, + ) + return False + if not self._verify_target_reached(label, req.pos, verify_orientation=verify_orientation): + return False + return True + + def _call_movel_relative_pos( + self, + label: str, + delta_mm_deg: list[float], + *, + velocity: float, + acceleration: float, + ref: int, + ) -> bool: + req = MoveLine.Request() + req.pos = [float(value) for value in delta_mm_deg] + req.vel = [float(velocity)] * 2 + req.acc = [float(acceleration)] * 2 + req.time = 0.0 + req.radius = 0.0 + req.ref = int(ref) + req.mode = MOVE_MODE_RELATIVE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + ref_label = "tool" if req.ref == DR_TOOL else "base" + self.get_logger().warn( + f"[뚜껑픽] 상대 이동 시작: {self._step_ko(label)} ref={ref_label} " + f"delta_mm=({req.pos[0]:.1f}, {req.pos[1]:.1f}, {req.pos[2]:.1f}) " + f"delta_rpy=({req.pos[3]:.1f}, {req.pos[4]:.1f}, {req.pos[5]:.1f}) deg" + ) + future = self._move_line_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} timed out or failed", + real_motion=True, + ) + return False + if not bool(result.success): + self._publish_status( + "failed", + error=f"{label} returned success=false", + real_motion=True, + ) + return False + return True + + def _call_movej_relative_pos( + self, + label: str, + delta_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 + if not self._move_joint_client.wait_for_service( + timeout_sec=max(float(self.get_parameter("move_timeout_sec").value), 0.0) + ): + self._publish_status( + "failed", + error=f"MoveJoint service unavailable: {self._move_joint_service_name}", + real_motion=False, + ) + return False + + before_joints = self._current_posj(timeout_sec=1.0) + req = MoveJoint.Request() + req.pos = [float(value) for value in delta_deg] + req.vel = float(velocity) + req.acc = float(acceleration) + req.time = 0.0 + req.radius = 0.0 + req.mode = MOVE_MODE_RELATIVE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + self._publish_status( + "lid_twist_joint_relative_start", + step=label, + delta_joints_deg=[round(value, 3) for value in req.pos], + before_joints_deg=( + [round(value, 3) for value in before_joints] if before_joints is not None else None + ), + velocity=round(float(velocity), 3), + acceleration=round(float(acceleration), 3), + real_motion=True, + ) + 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} timed out or failed", + real_motion=True, + ) + return False + if not bool(result.success): + self._publish_status( + "failed", + error=f"{label} returned success=false", + real_motion=True, + ) + return False + after_joints = self._current_posj(timeout_sec=1.0) + self._publish_status( + "lid_twist_joint_relative_done", + step=label, + delta_joints_deg=[round(value, 3) for value in req.pos], + after_joints_deg=( + [round(value, 3) for value in after_joints] if after_joints is not None else None + ), + real_motion=True, + ) + return True + + def _pose_to_dsr_pos(self, pose) -> list[float]: + return self._pose_to_dsr_pos_with_rz(pose, self._pick_rz_deg(pose)) + + def _pose_to_dsr_pos_with_rz(self, pose, rz_deg: float) -> list[float]: + return [ + float(pose.position.x) * 1000.0, + float(pose.position.y) * 1000.0, + float(pose.position.z) * 1000.0, + float(self.get_parameter("rx").value), + float(self.get_parameter("ry").value), + 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] + + def _pick_rz_selection(self, pose) -> tuple[float, dict]: + fixed_rz = float(self.get_parameter("rz").value) + yaw_x = self._pose_axis_yaw_deg(pose, "x") + yaw_y = self._pose_axis_yaw_deg(pose, "y") + fields = { + "pick_yaw_enabled": False, + "pick_rz_deg": round(fixed_rz, 3), + "lid_pose_x_yaw_deg": self._round_or_none(yaw_x, 3), + "lid_pose_y_yaw_deg": self._round_or_none(yaw_y, 3), + } + if not bool(self.get_parameter("use_lid_pose_yaw_for_pick").value): + return fixed_rz, fields + axis = self._lid_pose_yaw_axis() + yaw = yaw_y if axis == "y" else yaw_x + fields.update( + { + "pick_yaw_enabled": True, + "pick_yaw_valid": False, + "lid_pose_yaw_axis": axis, + "fixed_rz_deg": round(fixed_rz, 3), + } + ) + if yaw is None: + self.get_logger().warn( + "[뚜껑픽] ArUco yaw 사용 요청이 있었지만 pose orientation이 유효하지 않아 " + f"기존 rz={fixed_rz:.1f} deg를 사용합니다" + ) + return fixed_rz, fields + offset_deg = float(self.get_parameter("lid_pose_yaw_offset_deg").value) + requested_rz = yaw + offset_deg + equivalence_deg = float(self.get_parameter("lid_pose_yaw_equivalence_deg").value) + selected_rz = self._nearest_equivalent_angle_deg( + requested_rz, + reference_deg=fixed_rz, + period_deg=equivalence_deg, + ) + fields.update( + { + "pick_yaw_valid": True, + "lid_pose_yaw_deg": round(yaw, 3), + "lid_pose_yaw_offset_deg": round(offset_deg, 3), + "lid_pose_yaw_equivalence_deg": round(equivalence_deg, 3), + "requested_pick_rz_deg": round(requested_rz, 3), + "pick_rz_deg": round(selected_rz, 3), + } + ) + return selected_rz, fields + + @staticmethod + def _nearest_equivalent_angle_deg( + angle_deg: float, + *, + reference_deg: float, + period_deg: float, + ) -> float: + if not ( + math.isfinite(angle_deg) + and math.isfinite(reference_deg) + and math.isfinite(period_deg) + and abs(period_deg) > 1e-9 + ): + return angle_deg + period = abs(period_deg) + turns = round((reference_deg - angle_deg) / period) + return angle_deg + turns * period + + @staticmethod + def _pose_axis_yaw_deg(pose, axis_name: str) -> float | None: + qx = float(pose.orientation.x) + qy = float(pose.orientation.y) + qz = float(pose.orientation.z) + qw = float(pose.orientation.w) + norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) + if not math.isfinite(norm) or norm <= 1e-12: + return None + qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm + axis = str(axis_name).strip().lower() + if axis == "y": + vector_x = 2.0 * (qx * qy - qz * qw) + vector_y = 1.0 - 2.0 * (qx * qx + qz * qz) + else: + vector_x = 1.0 - 2.0 * (qy * qy + qz * qz) + vector_y = 2.0 * (qx * qy + qz * qw) + if abs(vector_x) <= 1e-12 and abs(vector_y) <= 1e-12: + return None + return math.degrees(math.atan2(vector_y, vector_x)) + + @staticmethod + def _round_or_none(value, digits: int): + if value is None: + return None + try: + numeric = float(value) + except (TypeError, ValueError): + return None + if not math.isfinite(numeric): + return None + return round(numeric, digits) + + 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() + if axis in {"y", "axis_y", "marker_y", "aruco_y"}: + return "y" + if axis in {"x", "axis_x", "marker_x", "aruco_x"}: + return "x" + self.get_logger().warn( + f"[뚜껑픽] {parameter_name}='{value}' 값이 유효하지 않아 {default_axis}축을 사용합니다" + ) + return default_axis + + def _precheck_ikin_steps(self, steps) -> bool: + if not bool(self.get_parameter("precheck_ikin").value): + return True + if self._ikin_client is None: + self._publish_status( + "failed", + error="Ikin client unavailable; refusing motion without IK precheck", + real_motion=False, + ) + return False + + 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 False + + for label, pose in steps: + req = Ikin.Request() + if isinstance(pose, (list, tuple)): + req.pos = [float(value) for value in pose] + else: + req.pos = self._pose_to_dsr_pos(pose) + 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 precheck failed", + target_mm_deg=[round(value, 3) for value in req.pos], + real_motion=False, + ) + return False + joints_deg = [float(value) for value in result.conv_posj] + self._publish_status( + "ikin_result", + step=label, + sol_space=req.sol_space, + joints_deg=[round(value, 3) for value in joints_deg], + target_mm_deg=[round(value, 3) for value in req.pos], + real_motion=True, + ) + return True + + def _verify_target_reached( + self, + label: str, + target_pos_mm_deg: list[float], + *, + verify_orientation: bool = False, + ) -> bool: + if not bool(self.get_parameter("verify_motion_reached").value): + return True + tolerance_m = max(float(self.get_parameter("motion_target_tolerance_m").value), 0.001) + orientation_tolerance_deg = max( + float(self.get_parameter("motion_target_orientation_tolerance_deg").value), + 0.1, + ) + timeout_sec = max(float(self.get_parameter("motion_verify_timeout_sec").value), 0.1) + deadline = time.monotonic() + timeout_sec + last_actual = None + last_distance_m = math.inf + last_orientation_error_deg = math.inf + while rclpy.ok() and time.monotonic() < deadline: + actual = self._current_posx(timeout_sec=2.0) + if actual is not None: + last_actual = actual + last_distance_m = math.sqrt( + sum((actual[index] - target_pos_mm_deg[index]) ** 2 for index in range(3)) + ) / 1000.0 + if verify_orientation: + last_orientation_error_deg = max( + abs(self._angle_delta_deg(actual[index], target_pos_mm_deg[index])) + for index in range(3, 6) + ) + orientation_ok = ( + not verify_orientation + or last_orientation_error_deg <= orientation_tolerance_deg + ) + if last_distance_m <= tolerance_m and orientation_ok: + actual_rpy = ( + [round(float(value), 3) for value in actual[3:6]] + if len(actual) >= 6 + else None + ) + target_rpy = [round(float(value), 3) for value in target_pos_mm_deg[3:6]] + rpy_error = ( + [ + round( + self._angle_delta_deg(actual[index], target_pos_mm_deg[index]), + 3, + ) + for index in range(3, 6) + ] + if len(actual) >= 6 + else None + ) + self._publish_status( + "motion_target_reached", + step=label, + distance_m=round(last_distance_m, 4), + tolerance_m=round(tolerance_m, 4), + target_rpy_deg=target_rpy, + actual_rpy_deg=actual_rpy, + rpy_error_deg=rpy_error, + orientation_error_deg=( + round(last_orientation_error_deg, 3) + if verify_orientation and math.isfinite(last_orientation_error_deg) + else None + ), + orientation_tolerance_deg=( + round(orientation_tolerance_deg, 3) + if verify_orientation + else None + ), + real_motion=True, + ) + return True + time.sleep(0.2) + + self._publish_status( + "failed", + error=f"{label} target was not reached after MoveLine accepted", + target_mm=[round(value, 3) for value in target_pos_mm_deg[:3]], + actual_mm=( + [round(value, 3) for value in last_actual[:3]] + if last_actual is not None + else None + ), + target_rpy_deg=[round(float(value), 3) for value in target_pos_mm_deg[3:6]], + actual_rpy_deg=( + [round(float(value), 3) for value in last_actual[3:6]] + if last_actual is not None and len(last_actual) >= 6 + else None + ), + rpy_error_deg=( + [ + round(self._angle_delta_deg(last_actual[index], target_pos_mm_deg[index]), 3) + for index in range(3, 6) + ] + if last_actual is not None and len(last_actual) >= 6 + else None + ), + distance_m=round(last_distance_m, 4) if math.isfinite(last_distance_m) else None, + tolerance_m=round(tolerance_m, 4), + orientation_error_deg=( + round(last_orientation_error_deg, 3) + if verify_orientation and math.isfinite(last_orientation_error_deg) + else None + ), + orientation_tolerance_deg=( + round(orientation_tolerance_deg, 3) + if verify_orientation + else None + ), + last_alarm=self._read_last_alarm(), + real_motion=True, + ) + return False + + def _try_lid_twist_sequence(self) -> bool: + if self._lid_twist_force_control_enabled(): + return self._try_lid_twist_force_sequence() + if self._preseat_periodic_enabled() and self._lid_twist_force_rotation_mode() == "j6": + return self._try_lid_twist_periodic_j6_sequence() + + twist_steps = self._lid_twist_steps() + if twist_steps is None: + return False + ikin_steps = [(label, pos) for label, pos, _velocity, _verify_orientation, _hold in twist_steps] + if not self._precheck_ikin_steps(ikin_steps): + return False + + acceleration = float(self.get_parameter("lid_twist_acceleration").value) + for label, pos, velocity, verify_orientation, hold_seconds in twist_steps: + if not self._call_movel_pos( + label, + pos, + velocity=velocity, + acceleration=acceleration, + verify_orientation=verify_orientation, + ): + return False + if hold_seconds > 0.0: + self._publish_status( + "lid_twist_holding", + step=label, + seconds=round(hold_seconds, 3), + real_motion=True, + ) + time.sleep(hold_seconds) + return True + + def _try_lid_twist_periodic_j6_sequence(self) -> bool: + plan = self._lid_twist_force_plan(force_control=False) + if plan is None: + return False + transfer_steps, turn_steps, release_step = plan + ikin_steps = [(label, pos) for label, pos, _velocity, _verify_orientation, _hold in transfer_steps] + if release_step[0] == "motion": + ikin_steps.append((release_step[1], release_step[2])) + if not self._precheck_ikin_steps(ikin_steps): + return False + required_clients = [ + (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( + "failed", + error=f"{name} client unavailable; dsr_msgs2 service type may be missing", + 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) + ): + self._publish_status( + "failed", + error=f"MoveJoint service unavailable: {self._move_joint_service_name}", + real_motion=False, + ) + return False + + acceleration = float(self.get_parameter("lid_twist_acceleration").value) + for label, pos, velocity, verify_orientation, hold_seconds in transfer_steps: + if not self._call_movel_pos( + label, + pos, + velocity=velocity, + acceleration=acceleration, + verify_orientation=verify_orientation, + ): + return False + if hold_seconds > 0.0: + self._publish_status( + "lid_twist_holding", + step=label, + seconds=round(hold_seconds, 3), + real_motion=True, + ) + time.sleep(hold_seconds) + + hold_before_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_before_turn").value), + 0.0, + ) + if hold_before_turn > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_periodic_settle", + seconds=round(hold_before_turn, 3), + real_motion=True, + ) + time.sleep(hold_before_turn) + 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: + if not self._execute_lid_twist_action(action, acceleration): + self._recover_lid_twist_abort(release_step, acceleration) + return False + hold_after_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_after_turn").value), + 0.0, + ) + if hold_after_turn > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_turn_complete", + seconds=round(hold_after_turn, 3), + real_motion=True, + ) + time.sleep(hold_after_turn) + if not self._open_gripper_after_lid_twist(): + return False + if not self._execute_lid_twist_action(release_step, acceleration): + return False + return True + + def _try_lid_twist_force_sequence(self) -> bool: + plan = self._lid_twist_force_plan() + if plan is None: + return False + transfer_steps, turn_steps, release_step = plan + ikin_steps = [(label, pos) for label, pos, _velocity, _verify_orientation, _hold in transfer_steps] + ikin_steps.extend( + (action[1], action[2]) + for action in turn_steps + if action[0] == "motion" + ) + if release_step[0] == "motion": + ikin_steps.append((release_step[1], release_step[2])) + if not self._precheck_ikin_steps(ikin_steps): + return False + + acceleration = float(self.get_parameter("lid_twist_acceleration").value) + for label, pos, velocity, verify_orientation, hold_seconds in transfer_steps: + if not self._call_movel_pos( + label, + pos, + velocity=velocity, + acceleration=acceleration, + verify_orientation=verify_orientation, + ): + return False + if hold_seconds > 0.0: + self._publish_status( + "lid_twist_holding", + step=label, + seconds=round(hold_seconds, 3), + real_motion=True, + ) + time.sleep(hold_seconds) + + force_enabled = False + force_released = True + try: + if not self._enable_lid_twist_force(): + self._recover_lid_twist_abort(release_step, acceleration) + return False + force_enabled = True + hold_before_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_before_turn").value), + 0.0, + ) + if hold_before_turn > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_force_settle", + seconds=round(hold_before_turn, 3), + real_motion=True, + ) + time.sleep(hold_before_turn) + if self._preseat_periodic_enabled(): + 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: + if not self._execute_lid_twist_action(action, acceleration): + self._recover_lid_twist_abort(release_step, acceleration) + return False + hold_after_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_after_turn").value), + 0.0, + ) + if hold_after_turn > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_turn_complete", + seconds=round(hold_after_turn, 3), + real_motion=True, + ) + time.sleep(hold_after_turn) + finally: + if force_enabled: + force_released = self._release_lid_twist_force() + if not force_released: + self._publish_status( + "failed", + error="failed to release force/compliance after lid twist; refusing release motion", + real_motion=True, + ) + return False + + if not self._open_gripper_after_lid_twist(): + return False + + if not self._execute_lid_twist_action(release_step, acceleration): + return False + return True + + def _execute_lid_twist_action(self, action, acceleration: float) -> bool: + kind = action[0] + if kind == "gripper": + if not self._call_lid_twist_gripper_action(action[1]): + return False + hold_seconds = float(action[2]) + if hold_seconds > 0.0: + self._publish_status( + "lid_twist_holding", + step=f"lid_twist_regrip_{action[1]}", + seconds=round(hold_seconds, 3), + real_motion=True, + ) + time.sleep(hold_seconds) + return True + if kind == "motion": + _kind, label, pos, velocity, verify_orientation, hold_seconds = action + if not self._call_movel_pos( + label, + pos, + velocity=velocity, + acceleration=acceleration, + verify_orientation=verify_orientation, + ): + return False + elif kind == "relative_motion": + _kind, label, delta, velocity, ref, hold_seconds = action + if not self._call_movel_relative_pos( + label, + delta, + velocity=velocity, + acceleration=acceleration, + ref=ref, + ): + return False + elif kind == "relative_joint": + _kind, label, delta, velocity, hold_seconds = action + if not self._call_movej_relative_pos( + label, + delta, + velocity=velocity, + acceleration=acceleration, + ): + return False + else: + self._publish_status( + "failed", + error=f"unsupported lid twist action kind: {kind}", + real_motion=False, + ) + return False + + if hold_seconds > 0.0: + self._publish_status( + "lid_twist_holding", + step=label, + seconds=round(hold_seconds, 3), + real_motion=True, + ) + time.sleep(hold_seconds) + return True + + def _recover_lid_twist_abort(self, release_step, acceleration: float) -> None: + self._publish_status( + "lid_twist_abort_recovery", + real_motion=True, + ) + self._release_lid_twist_force() + self._execute_lid_twist_action(release_step, acceleration) + + def _lid_twist_force_plan(self, *, force_control: bool = True): + target = self._lid_twist_target_pos() + if target is None: + return None + rz_delta_deg = float(self.get_parameter("lid_twist_rz_delta_deg").value) + turn_step_deg = abs(float(self.get_parameter("lid_twist_turn_step_deg").value)) + transfer_clearance_m = max( + 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) + transfer_max_z_m = float(self.get_parameter("lid_twist_transfer_max_z_m").value) + if not math.isfinite(rz_delta_deg) or abs(rz_delta_deg) <= 1e-9: + self._publish_status( + "failed", + error="lid_twist_rz_delta_deg must be finite and non-zero for force lid twist", + real_motion=False, + ) + return None + if not math.isfinite(turn_step_deg) or turn_step_deg <= 0.0: + self._publish_status( + "failed", + error="lid_twist_turn_step_deg must be positive", + real_motion=False, + ) + return None + + 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), + 0.0, + ) + turn_action_plan = self._lid_twist_force_turn_actions( + target, + rz_delta_deg, + turn_step_deg, + turn_velocity, + hold_after_turn, + ) + if turn_action_plan is None: + return None + turn_steps, turned, closed_turn_deg = turn_action_plan + turn_motion_steps = [ + action + for action in turn_steps + if action[0] in ("motion", "relative_motion") + ] + absolute_turn_motion_steps = [action for action in turn_steps if action[0] == "motion"] + release = turned.copy() + release_delta_z = release_lift_m * 1000.0 + if transfer_clearance_m > 0.0: + release[2] = target_high[2] + release_delta_z = target_high[2] - target[2] + else: + release[2] += release_lift_m * 1000.0 + 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), + ]) + validation_steps.extend( + (label, pos, max_z_m) + for _kind, label, pos, _velocity, _verify_orientation, _hold in absolute_turn_motion_steps + ) + for label, pos, upper_z_m in validation_steps: + z_m = pos[2] / 1000.0 + if z_m < min_z_m or z_m > upper_z_m: + self._publish_status( + "failed", + error=( + f"{label} z={z_m:.3f} outside " + f"[{min_z_m:.3f}, {upper_z_m:.3f}]" + ), + target_mm=[round(value, 3) for value in pos[:3]], + real_motion=False, + ) + return None + + transfer_velocity = float(self.get_parameter("lid_twist_transfer_velocity").value) + press_velocity = float(self.get_parameter("lid_twist_press_velocity").value) + revolution = abs(closed_turn_deg) / 360.0 + self._publish_status( + "lid_twist_planned", + target_mm_deg=[round(value, 3) for value in target], + target_high_mm_deg=[round(value, 3) for value in target_high], + pressed_mm_deg=[round(value, 3) for value in target], + turned_mm_deg=[round(value, 3) for value in turned], + press_down_m=0.0, + rz_delta_deg=round(rz_delta_deg, 3), + turn_step_deg=round(turn_step_deg, 3), + turn_count=len(turn_motion_steps) if turn_motion_steps else 1, + transfer_clearance_m=round(transfer_clearance_m, 4), + force_control=bool(force_control), + force_rotation_mode=self._lid_twist_force_rotation_mode(), + revolution=round(revolution, 3), + regrip_cycles=1, + post_turn_gripper_open=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, + ), + preseat_periodic_repeat=int(self.get_parameter("lid_twist_preseat_periodic_repeat").value), + real_motion=True, + ) + + transfer_steps = [] + if transfer_clearance_m > 0.0: + transfer_steps.append(("lid_twist_transfer_high", target_high, transfer_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", + [0.0, 0.0, release_delta_z, 0.0, 0.0, 0.0], + transfer_velocity, + DR_BASE, + 0.0, + ) + return transfer_steps, turn_steps, release_step + + def _lid_twist_force_turn_actions( + self, + target: list[float], + rz_delta_deg: float, + turn_step_deg: float, + turn_velocity: float, + hold_after_turn: float, + ): + rotation_mode = self._lid_twist_force_rotation_mode() + turn_deltas = self._lid_twist_turn_delta_steps(rz_delta_deg, turn_step_deg) + if rotation_mode == "j6": + actions = [ + ( + "relative_joint", + self._lid_twist_turn_label(index, len(turn_deltas), rz_delta_deg), + [0.0, 0.0, 0.0, 0.0, 0.0, delta], + turn_velocity, + hold_after_turn if index == len(turn_deltas) else 0.0, + ) + for index, delta in enumerate(turn_deltas, start=1) + ] + else: + actions = [ + ( + "relative_motion", + self._lid_twist_turn_label(index, len(turn_deltas), rz_delta_deg), + [0.0, 0.0, 0.0, 0.0, 0.0, delta], + turn_velocity, + DR_TOOL, + hold_after_turn if index == len(turn_deltas) else 0.0, + ) + for index, delta in enumerate(turn_deltas, start=1) + ] + turned = target.copy() + turned[5] += sum(turn_deltas) + return actions, turned, sum(turn_deltas) + + def _enable_lid_twist_force(self) -> bool: + if not self._force_twist_clients_available(): + return False + stiffness = self._lid_twist_compliance_stiffness() + force_ref = self._lid_twist_force_ref() + down_force_n = max(float(self.get_parameter("lid_twist_down_force_n").value), 0.0) + if down_force_n <= 0.0: + self._publish_status( + "failed", + error="lid_twist_down_force_n must be positive for force lid twist", + real_motion=False, + ) + return False + + task_req = TaskComplianceCtrl.Request() + task_req.stx = stiffness + task_req.ref = force_ref + task_req.time = min( + max(float(self.get_parameter("lid_twist_force_settle_seconds").value), 0.0), + 1.0, + ) + force_timeout_sec = self._lid_twist_force_timeout_sec() + if not self._call_bool_service( + self._task_compliance_client, + task_req, + "lid_twist_task_compliance", + self._task_compliance_service_name, + force_timeout_sec, + ): + return False + + force_req = SetDesiredForce.Request() + force_req.fd = [0.0, 0.0, -down_force_n, 0.0, 0.0, 0.0] + force_req.dir = [0, 0, 1, 0, 0, 0] + force_req.ref = force_ref + force_req.time = min( + max(float(self.get_parameter("lid_twist_force_settle_seconds").value), 0.0), + 1.0, + ) + force_req.mod = DR_FC_MOD_REL + if not self._call_bool_service( + self._set_desired_force_client, + force_req, + "lid_twist_set_force", + self._set_desired_force_service_name, + force_timeout_sec, + ): + self._release_lid_twist_force() + return False + + self._publish_status( + "lid_twist_force_enabled", + down_force_n=round(down_force_n, 3), + ref="base" if force_ref == DR_BASE else "tool", + stiffness=[round(value, 3) for value in stiffness], + real_motion=True, + ) + return True + + 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", + error="MovePeriodic client unavailable; cannot execute pre-seat periodic motion", + 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 + + period_sec = max( + float(self.get_parameter("lid_twist_preseat_periodic_period_sec").value), + 0.01, + ) + acc_time_sec = max( + float(self.get_parameter("lid_twist_preseat_periodic_acc_time_sec").value), + 0.0, + ) + repeat = max(int(self.get_parameter("lid_twist_preseat_periodic_repeat").value), 1) + amp = self._preseat_periodic_amp() + periodic = [period_sec] * 6 + + req = MovePeriodic.Request() + req.amp = amp + req.periodic = periodic + req.acc = acc_time_sec + req.repeat = repeat + req.ref = self._preseat_periodic_ref() + req.sync_type = int(sync_type) + self._publish_status( + "lid_twist_preseat_periodic_start", + amp=[round(value, 3) for value in amp], + period_sec=round(period_sec, 3), + 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, + label, + self._move_periodic_service_name, + float(self.get_parameter("move_timeout_sec").value), + ) + + def _preseat_periodic_enabled(self) -> bool: + return bool(self.get_parameter("lid_twist_preseat_periodic_before_turn").value) + + def _preseat_periodic_amp(self) -> list[float]: + return [ + float(self.get_parameter("lid_twist_preseat_periodic_x_amp_mm").value), + float(self.get_parameter("lid_twist_preseat_periodic_y_amp_mm").value), + float(self.get_parameter("lid_twist_preseat_periodic_z_amp_mm").value), + float(self.get_parameter("lid_twist_preseat_periodic_rx_amp_deg").value), + float(self.get_parameter("lid_twist_preseat_periodic_ry_amp_deg").value), + 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": + return DR_BASE + return DR_TOOL + + def _release_lid_twist_force(self) -> bool: + ok = True + force_timeout_sec = self._lid_twist_force_timeout_sec() + release_time = min( + max(float(self.get_parameter("lid_twist_force_release_time").value), 0.0), + 1.0, + ) + if self._release_force_client is not None: + req = ReleaseForce.Request() + req.time = release_time + ok = self._call_bool_service( + self._release_force_client, + req, + "lid_twist_release_force", + self._release_force_service_name, + force_timeout_sec, + publish_failure=False, + ) and ok + if self._release_compliance_client is not None: + ok = self._call_bool_service( + self._release_compliance_client, + ReleaseComplianceCtrl.Request(), + "lid_twist_release_compliance", + self._release_compliance_service_name, + force_timeout_sec, + publish_failure=False, + ) and ok + self._publish_status( + "lid_twist_force_released", + success=ok, + real_motion=True, + ) + return ok + + def _call_lid_twist_gripper_action(self, command: str) -> bool: + if not bool(self.get_parameter("enable_gripper_service_calls").value): + self._publish_status( + "failed", + error="lid twist gripper action requires enable_gripper_service_calls:=true", + real_motion=False, + ) + return False + targets = self._gripper_targets() + if targets is None: + return False + timeout_sec = float(self.get_parameter("gripper_wait_timeout_sec").value) + if not self._gripper_client.wait_for_service(timeout_sec=max(timeout_sec, 0.0)): + self._publish_status( + "failed", + error=f"gripper service unavailable: {self.get_parameter('gripper_set_service').value}", + real_motion=False, + ) + return False + preopen_width, grasp_width, force_n = targets + if command == "preopen": + return self._call_gripper_sync("preopen", preopen_width, force_n) + if command == "grasp": + return self._call_gripper_sync("grasp", grasp_width, force_n) + self._publish_status( + "failed", + error=f"unsupported lid twist gripper action: {command}", + real_motion=False, + ) + return False + + def _open_gripper_after_lid_twist(self) -> bool: + if not self._call_lid_twist_gripper_action("preopen"): + return False + gripper_wait = max( + float(self.get_parameter("lid_twist_regrip_gripper_wait_sec").value), + 0.0, + ) + if gripper_wait > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_final_preopen", + seconds=round(gripper_wait, 3), + real_motion=True, + ) + time.sleep(gripper_wait) + return True + + def _force_twist_clients_available(self) -> bool: + required = [ + (self._task_compliance_client, self._task_compliance_service_name), + (self._set_desired_force_client, self._set_desired_force_service_name), + (self._release_force_client, self._release_force_service_name), + (self._release_compliance_client, self._release_compliance_service_name), + ] + rotation_mode = self._lid_twist_force_rotation_mode() + 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" 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), + ]) + timeout_sec = self._lid_twist_force_timeout_sec() + for client, name in required: + if client is None: + self._publish_status( + "failed", + error=f"{name} client unavailable; dsr_msgs2 service type may be missing", + real_motion=False, + ) + return False + if not client.wait_for_service( + timeout_sec=timeout_sec, + ): + self._publish_status( + "failed", + error=f"{name} service unavailable", + real_motion=False, + ) + return False + return True + + def _call_bool_service( + self, + client, + request, + label: str, + service: str, + timeout_sec: float, + *, + publish_failure: bool = True, + ) -> bool: + if client is None: + if publish_failure: + self._publish_status( + "failed", + error=f"{label} client unavailable: {service}", + real_motion=False, + ) + return False + self._publish_status( + "lid_twist_service_call", + step=label, + service=service, + timeout_sec=round(timeout_sec, 3), + real_motion=True, + ) + future = client.call_async(request) + result = self._wait_for_future(future, label, timeout_sec) + if result is None or not bool(result.success): + if publish_failure: + self._publish_status( + "failed", + error=f"{label} returned success=false", + service=service, + real_motion=True, + ) + return False + return True + + def _lid_twist_force_timeout_sec(self) -> float: + return max(float(self.get_parameter("lid_twist_force_service_timeout_sec").value), 0.1) + + def _lid_twist_force_control_enabled(self) -> bool: + return bool(self.get_parameter("lid_twist_use_force_control").value) or bool( + self.get_parameter("lid_twist_use_force_spiral").value + ) + + def _lid_twist_compliance_stiffness(self) -> list[float]: + return [ + float(self.get_parameter("lid_twist_compliance_x_stiffness").value), + float(self.get_parameter("lid_twist_compliance_y_stiffness").value), + float(self.get_parameter("lid_twist_compliance_z_stiffness").value), + float(self.get_parameter("lid_twist_compliance_rx_stiffness").value), + float(self.get_parameter("lid_twist_compliance_ry_stiffness").value), + float(self.get_parameter("lid_twist_compliance_rz_stiffness").value), + ] + + def _lid_twist_force_ref(self) -> int: + value = str(self.get_parameter("lid_twist_force_ref").value).strip().lower() + if value == "tool": + return DR_TOOL + return DR_BASE + + def _lid_twist_force_rotation_mode(self) -> str: + value = str(self.get_parameter("lid_twist_force_rotation_mode").value).strip().lower() + if value in {"j6", "joint", "joint6", "movej", "move_joint"}: + return "j6" + return "movel" + + def _lid_twist_steps(self): + target = self._lid_twist_target_pos() + if target is None: + return None + press_down_m = max(float(self.get_parameter("lid_twist_press_down_m").value), 0.0) + rz_delta_deg = float(self.get_parameter("lid_twist_rz_delta_deg").value) + turn_step_deg = abs(float(self.get_parameter("lid_twist_turn_step_deg").value)) + transfer_clearance_m = max( + float(self.get_parameter("lid_twist_transfer_clearance_m").value), + 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) + transfer_max_z_m = float(self.get_parameter("lid_twist_transfer_max_z_m").value) + if not math.isfinite(rz_delta_deg): + self._publish_status( + "failed", + error="lid_twist_rz_delta_deg must be finite", + real_motion=False, + ) + return None + if not math.isfinite(turn_step_deg) or turn_step_deg <= 0.0: + self._publish_status( + "failed", + error="lid_twist_turn_step_deg must be positive", + real_motion=False, + ) + return None + + target_high = target.copy() + target_high[2] += transfer_clearance_m * 1000.0 + pressed = target.copy() + pressed[2] -= press_down_m * 1000.0 + turn_targets = self._lid_twist_turn_targets(pressed, rz_delta_deg, turn_step_deg) + turned = turn_targets[-1] if turn_targets else pressed.copy() + release = turned.copy() + release[2] += release_lift_m * 1000.0 + + validation_steps = [] + if transfer_clearance_m > 0.0: + validation_steps.append(("lid_twist_transfer_high", target_high)) + validation_steps.extend([ + ("lid_twist_transfer", target), + ("lid_twist_press", pressed), + ("lid_twist_release", release), + ]) + validation_steps.extend( + (self._lid_twist_turn_label(index, len(turn_targets), rz_delta_deg), pos) + for index, pos in enumerate(turn_targets, start=1) + ) + for label, pos in validation_steps: + z_m = pos[2] / 1000.0 + upper_z_m = transfer_max_z_m if label == "lid_twist_transfer_high" else max_z_m + if z_m < min_z_m or z_m > upper_z_m: + self._publish_status( + "failed", + error=( + f"{label} z={z_m:.3f} outside " + f"[{min_z_m:.3f}, {upper_z_m:.3f}]" + ), + target_mm=[round(value, 3) for value in pos[:3]], + real_motion=False, + ) + return None + + transfer_velocity = float(self.get_parameter("lid_twist_transfer_velocity").value) + press_velocity = float(self.get_parameter("lid_twist_press_velocity").value) + turn_velocity = float(self.get_parameter("lid_twist_turn_velocity").value) + hold_before_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_before_turn").value), + 0.0, + ) + hold_after_turn = max( + float(self.get_parameter("lid_twist_hold_seconds_after_turn").value), + 0.0, + ) + self._publish_status( + "lid_twist_planned", + target_mm_deg=[round(value, 3) for value in target], + target_high_mm_deg=[round(value, 3) for value in target_high], + pressed_mm_deg=[round(value, 3) for value in pressed], + turned_mm_deg=[round(value, 3) for value in turned], + press_down_m=round(press_down_m, 4), + rz_delta_deg=round(rz_delta_deg, 3), + turn_step_deg=round(turn_step_deg, 3), + turn_count=len(turn_targets), + transfer_clearance_m=round(transfer_clearance_m, 4), + real_motion=True, + ) + steps = [] + if transfer_clearance_m > 0.0: + steps.append(("lid_twist_transfer_high", target_high, transfer_velocity, True, 0.0)) + steps.extend([ + ("lid_twist_transfer", target, press_velocity, True, 0.0), + ("lid_twist_press", pressed, press_velocity, True, hold_before_turn), + ]) + steps.extend( + ( + self._lid_twist_turn_label(index, len(turn_targets), rz_delta_deg), + pos, + turn_velocity, + True, + hold_after_turn if index == len(turn_targets) else 0.0, + ) + for index, pos in enumerate(turn_targets, start=1) + ) + steps.append(("lid_twist_release", release, press_velocity, True, 0.0)) + return steps + + @staticmethod + def _lid_twist_turn_targets(pressed_pos: list[float], total_delta_deg: float, step_deg: float) -> list[list[float]]: + if abs(total_delta_deg) <= 1e-9: + return [] + direction = 1.0 if total_delta_deg > 0.0 else -1.0 + remaining = abs(total_delta_deg) + cumulative = 0.0 + targets: list[list[float]] = [] + while remaining > 1e-9: + increment = min(step_deg, remaining) + cumulative += direction * increment + target = pressed_pos.copy() + target[5] = pressed_pos[5] + cumulative + targets.append(target) + remaining -= increment + return targets + + @staticmethod + def _lid_twist_turn_delta_steps(total_delta_deg: float, step_deg: float) -> list[float]: + if abs(total_delta_deg) <= 1e-9: + return [] + direction = 1.0 if total_delta_deg > 0.0 else -1.0 + remaining = abs(total_delta_deg) + deltas: list[float] = [] + while remaining > 1e-9: + increment = min(step_deg, remaining) + deltas.append(direction * increment) + remaining -= increment + return deltas + + @staticmethod + def _lid_twist_turn_label(index: int, total: int, delta_deg: float) -> str: + direction = "clockwise" if delta_deg > 0.0 else "counterclockwise" + return f"lid_twist_turn_{direction}_{index:02d}_of_{total:02d}" + + @staticmethod + def _lid_twist_cycle_turn_label( + phase: str, + cycle_index: int, + cycle_total: int, + step_index: int, + step_total: int, + delta_deg: float, + ) -> str: + direction = "clockwise" if delta_deg > 0.0 else "counterclockwise" + return ( + f"lid_twist_{phase}_{direction}_" + f"c{cycle_index:02d}_of_{cycle_total:02d}_" + f"{step_index:02d}_of_{step_total:02d}" + ) + + def _lid_twist_target_pos(self) -> list[float] | None: + values = [ + float(self.get_parameter("lid_twist_target_x_m").value) * 1000.0, + float(self.get_parameter("lid_twist_target_y_m").value) * 1000.0, + float(self.get_parameter("lid_twist_target_z_m").value) * 1000.0, + float(self.get_parameter("lid_twist_rx").value), + float(self.get_parameter("lid_twist_ry").value), + float(self.get_parameter("lid_twist_rz").value), + ] + if not all(math.isfinite(value) for value in values): + self._publish_status( + "failed", + error=( + "lid twist target is required before post-grasp rotation; set " + "lid_twist_target_x_m/y_m/z_m and lid_twist_rx/ry/rz from measured teach point" + ), + real_motion=False, + ) + return None + return values + + def _current_posj(self, timeout_sec: float) -> list[float] | None: + if self._current_posj_client is None: + return None + req = GetCurrentPosj.Request() + future = self._current_posj_client.call_async(req) + result = self._wait_for_future(future, "get_current_posj", timeout_sec) + if result is None or not bool(result.success): + return None + values = [float(value) for value in result.pos] + if len(values) < 6: + return None + return values[:6] + + def _current_posx(self, timeout_sec: float) -> list[float] | None: + if self._current_posx_client is None: + return None + req = GetCurrentPosx.Request() + req.ref = DR_BASE + future = self._current_posx_client.call_async(req) + result = self._wait_for_future(future, "get_current_posx", timeout_sec) + if result is None or not bool(result.success) or not result.task_pos_info: + return None + values = [float(value) for value in result.task_pos_info[0].data] + if len(values) < 6: + return None + return values[:6] + + def _read_last_alarm(self) -> dict | None: + if self._last_alarm_client is None: + return None + if not self._last_alarm_client.wait_for_service(timeout_sec=0.5): + return None + future = self._last_alarm_client.call_async(GetLastAlarm.Request()) + result = self._wait_for_future(future, "get_last_alarm", 1.0) + if result is None or not bool(result.success): + return None + alarm = result.log_alarm + return { + "level": int(alarm.level), + "group": int(alarm.group), + "index": int(alarm.index), + "param": list(alarm.param), + } + + def _try_gripper_sequence(self) -> None: + targets = self._gripper_targets() + if targets is None: + return + preopen_width, grasp_width, force_n = targets + timeout_sec = float(self.get_parameter("gripper_wait_timeout_sec").value) + if not self._gripper_client.wait_for_service(timeout_sec=max(timeout_sec, 0.0)): + self._publish_status( + "failed", + error=f"gripper service unavailable: {self.get_parameter('gripper_set_service').value}", + real_motion=False, + ) + return + + self._call_gripper("preopen", preopen_width, force_n) + self._publish_status( + "gripper_preopen_requested", + width_m=preopen_width, + force_n=force_n, + real_motion=False, + note="RG2 request only; no Doosan motion was executed", + ) + self._call_gripper("grasp", grasp_width, force_n) + self._publish_status( + "gripper_grasp_requested", + width_m=grasp_width, + force_n=force_n, + real_motion=False, + note="RG2 request only; no Doosan motion was executed", + ) + + def _gripper_targets(self): + preopen_width = float(self.get_parameter("gripper_preopen_width_m").value) + grasp_width = float(self.get_parameter("gripper_grasp_width_m").value) + force_n = float(self.get_parameter("gripper_force_n").value) + if not all(math.isfinite(value) and value >= 0.0 for value in (preopen_width, grasp_width)): + self._publish_status( + "failed", + error=( + "measured gripper_preopen_width_m and gripper_grasp_width_m " + "are required before RG2 service calls" + ), + real_motion=False, + ) + return None + if not math.isfinite(force_n) or force_n <= 0.0: + self._publish_status( + "failed", + error="measured gripper_force_n is required before RG2 service calls", + real_motion=False, + ) + return None + return preopen_width, grasp_width, force_n + + def _call_gripper(self, command: str, width_m: float, force_n: float) -> None: + req = SetGripper.Request() + req.command = command + req.width_m = float(width_m) + req.force_n = float(force_n) + future = self._gripper_client.call_async(req) + future.add_done_callback(lambda done: self._on_gripper_done(command, done)) + + def _call_gripper_sync( + self, + command: str, + width_m: float, + force_n: float, + *, + publish_failure: bool = True, + ) -> bool: + req = SetGripper.Request() + req.command = command + req.width_m = float(width_m) + req.force_n = float(force_n) + future = self._gripper_client.call_async(req) + result = self._wait_for_future( + future, + f"gripper_{command}", + float(self.get_parameter("gripper_wait_timeout_sec").value), + ) + if result is None: + if publish_failure: + self._publish_status( + "failed", + error=f"RG2 {command} request timed out or failed", + real_motion=True, + ) + return False + self._publish_status( + "gripper_result", + command=command, + success=bool(result.success), + message=str(result.message), + real_motion=True, + ) + return bool(result.success) + + def _wait_for_future(self, future, label: str, timeout_sec: float): + deadline = time.monotonic() + max(timeout_sec, 0.0) + while rclpy.ok() and not future.done(): + if time.monotonic() > deadline: + self.get_logger().error( + f"[뚜껑픽] 실패: {self._step_ko(label)} 서비스 응답 대기 " + f"{timeout_sec:.1f}초 초과" + ) + return None + time.sleep(0.01) + if not future.done(): + return None + if future.exception() is not None: + self.get_logger().error( + f"[뚜껑픽] 실패: {self._step_ko(label)} 서비스 예외: {future.exception()}" + ) + return None + return future.result() + + def _on_gripper_done(self, command: str, future) -> None: + try: + result = future.result() + except Exception as exc: + self._publish_status( + "failed", + error=f"RG2 {command} request failed: {exc}", + real_motion=False, + ) + return + self._publish_status( + "gripper_result", + command=command, + success=bool(result.success), + message=str(result.message), + real_motion=False, + ) + + def _config(self) -> LidGripConfig: + return LidGripConfig( + approach_offset_m=float(self.get_parameter("approach_offset_m").value), + lift_offset_m=float(self.get_parameter("lift_offset_m").value), + min_approach_z_m=float(self.get_parameter("min_approach_z_m").value), + surface_offset_m=float(self.get_parameter("surface_offset_m").value), + offset_axis=str(self.get_parameter("offset_axis").value), + tcp_grasp_offset_x_m=float(self.get_parameter("tcp_grasp_offset_x_m").value), + tcp_grasp_offset_y_m=float(self.get_parameter("tcp_grasp_offset_y_m").value), + tcp_grasp_offset_z_m=float(self.get_parameter("tcp_grasp_offset_z_m").value), + min_grasp_z_m=float(self.get_parameter("min_grasp_z_m").value), + max_grasp_z_m=float(self.get_parameter("max_grasp_z_m").value), + ) + + def _publish_status(self, status: str, **fields) -> None: + payload = {"status": status, "node": "lid_grip_planner_node"} + payload.update(fields) + text = json.dumps(payload, sort_keys=True) + msg = String() + msg.data = text + self._status_pub.publish(msg) + if not self._should_log_status(status, fields): + return + if bool(self.get_parameter("log_korean_status").value): + message = self._human_status_text(status, fields) + if status == "failed": + self.get_logger().error(message) + else: + self.get_logger().info(message) + if not bool(self.get_parameter("log_json_status").value): + return + if status == "failed": + self.get_logger().error(text) + else: + self.get_logger().info(text) + + def _should_log_status(self, status: str, fields: dict) -> bool: + request_source = fields.get("request_source") + if request_source == "pose" and not bool(self.get_parameter("log_pose_plans").value): + return False + return True + + def _human_status_text(self, status: str, fields: dict) -> str: + if status == "trigger_received": + hardware = "ON" if fields.get("hardware_armed") else "OFF" + return f"[뚜껑픽] 1/6 p 입력 수신: hardware_armed={hardware}" + if status == "planned": + grasp = fields.get("grasp", {}) + approach = fields.get("approach", {}) + lift = fields.get("lift", {}) + motion = "ON" if fields.get("motion_allowed") else "OFF" + rz_text = "" + if fields.get("pick_rz_deg") is not None: + rz_text = f", pick_rz={fields.get('pick_rz_deg')}deg" + if fields.get("pick_yaw_enabled"): + rz_text += ( + f", axis={fields.get('lid_pose_yaw_axis')}" + f", aruco_yaw={fields.get('lid_pose_yaw_deg')}deg" + f", yaw_x={fields.get('lid_pose_x_yaw_deg')}deg" + f", yaw_y={fields.get('lid_pose_y_yaw_deg')}deg" + ) + return ( + "[뚜껑픽] 2/6 목표 계산 완료: " + f"approach z={self._fmt_coord(approach, 'z')} m, " + f"grasp z={self._fmt_coord(grasp, 'z')} m, " + f"lift z={self._fmt_coord(lift, 'z')} m, motion={motion}{rz_text}" + ) + if status == "ikin_result": + target = fields.get("target_mm_deg", []) + return ( + f"[뚜껑픽] IK 확인 완료: {self._step_ko(str(fields.get('step', '')))} " + f"target_mm={self._fmt_target_mm(target)}" + ) + if status == "motion_target_reached": + target_rpy = fields.get("target_rpy_deg") + actual_rpy = fields.get("actual_rpy_deg") + rpy_error = fields.get("rpy_error_deg") + rpy_text = "" + if target_rpy is not None and actual_rpy is not None: + rpy_text = ( + f" target_rpy={target_rpy} actual_rpy={actual_rpy} " + f"rpy_error={rpy_error}" + ) + return ( + f"[뚜껑픽] 이동 도착 확인: {self._step_ko(str(fields.get('step', '')))} " + f"오차={fields.get('distance_m')} m{rpy_text}" + ) + if status == "gripper_result": + command = str(fields.get("command", "")) + success = "성공" if fields.get("success") else "실패" + return f"[뚜껑픽] 그리퍼 {self._gripper_command_ko(command)} {success}: {fields.get('message', '')}" + if status == "settling_before_grasp": + return f"[뚜껑픽] 파지 위치에서 정지 대기: {fields.get('seconds')}초 후 그리퍼를 닫습니다" + if status == "visual_refine_collecting": + return ( + "[뚜껑보정] approach 위치에서 ArUco 재관측 시작: " + f"samples={fields.get('sample_count')} " + f"timeout={fields.get('timeout_sec')}초 " + f"axis={fields.get('axis')}" + ) + if status == "visual_refine_result": + return ( + "[뚜껑보정] 재관측 완료: " + f"samples={fields.get('samples')} " + f"xy_std={fields.get('position_std_m')}m " + f"yaw_std={fields.get('yaw_std_deg')}deg " + f"mean=({fields.get('mean_x')}, {fields.get('mean_y')}) " + f"axis={fields.get('axis')} " + f"yaw={fields.get('mean_yaw_deg')}deg " + f"pick_rz={fields.get('pick_rz_deg')}deg " + f"(x: yaw={fields.get('mean_yaw_x_deg')}deg/rz={fields.get('pick_rz_x_deg')}deg, " + f"y: yaw={fields.get('mean_yaw_y_deg')}deg/rz={fields.get('pick_rz_y_deg')}deg)" + ) + if status == "visual_refine_fallback": + return ( + "[뚜껑보정] 재관측 실패, 기존 좌표로 진행: " + f"reason={fields.get('reason')} " + f"samples={fields.get('samples')} " + 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 계속 진행: " + f"wait={fields.get('seconds')}초" + ) + if status == "lid_twist_planned": + target_high = fields.get("target_high_mm_deg", []) + pressed = fields.get("pressed_mm_deg", []) + turned = fields.get("turned_mm_deg", []) + mode = ( + f"force_{fields.get('force_rotation_mode')}" + if fields.get("force_control") + else "position" + ) + return ( + f"[뚜껑회전] 목표 계산 완료({mode}): " + f"safe target_mm={self._fmt_target_mm(target_high)} " + f"press target_mm={self._fmt_target_mm(pressed)} " + f"turn_rz={self._fmt_rz(turned)} deg " + f"turn_count={fields.get('turn_count')} " + f"step={fields.get('turn_step_deg')} deg " + f"clearance={fields.get('transfer_clearance_m')} m " + f"press_down={fields.get('press_down_m')} m " + f"rz_delta={fields.get('rz_delta_deg')} deg" + ) + if status == "lid_twist_force_enabled": + return ( + "[뚜껑회전] 힘 제어 ON: " + f"down_force={fields.get('down_force_n')} N " + f"ref={fields.get('ref')} stiffness={fields.get('stiffness')}" + ) + if status == "lid_twist_service_call": + return ( + f"[뚜껑회전] {self._step_ko(str(fields.get('step', '')))} " + f"서비스 호출: {fields.get('service')} " + f"timeout={fields.get('timeout_sec')}초" + ) + if status == "lid_twist_abort_recovery": + return "[뚜껑회전] 회전 실패 복구: 힘 해제 후 안전 상승을 시도합니다" + if status == "lid_twist_preseat_periodic_start": + return ( + "[뚜껑회전] pre-seat periodic 비틀림 안착 시작: " + f"amp={fields.get('amp')} period={fields.get('period_sec')}초 " + 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"{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')}" + ) + if status == "lid_twist_joint_relative_done": + after = fields.get("after_joints_deg") + 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"{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": + success = "성공" if fields.get("success") else "실패" + return f"[뚜껑회전] 힘 제어 해제 {success}" + if status == "lid_twist_holding": + return ( + f"[뚜껑회전] {self._step_ko(str(fields.get('step', '')))} " + f"정지 대기: {fields.get('seconds')}초" + ) + if status == "gripper_preopen_requested": + return ( + "[뚜껑픽] 그리퍼 preopen 요청: " + f"width={fields.get('width_m')} m force={fields.get('force_n')} N" + ) + if status == "gripper_grasp_requested": + return ( + "[뚜껑픽] 그리퍼 grasp 요청: " + f"width={fields.get('width_m')} m force={fields.get('force_n')} N" + ) + if status == "motion_sequence_requested": + 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": + detail = str(fields.get("error", "unknown")) + step = fields.get("step") + if step: + detail = f"{self._step_ko(str(step))}: {detail}" + target = fields.get("target_mm") or fields.get("target_mm_deg") + actual = fields.get("actual_mm") + extra = "" + if target is not None: + extra += f" target={target}" + if actual is not None: + extra += f" actual={actual}" + if fields.get("distance_m") is not None: + extra += f" distance={fields.get('distance_m')}m" + if fields.get("tolerance_m") is not None: + extra += f" tolerance={fields.get('tolerance_m')}m" + if fields.get("orientation_error_deg") is not None: + extra += f" orientation_error={fields.get('orientation_error_deg')}deg" + return f"[뚜껑픽] 실패: {detail}{extra}" + return f"[뚜껑픽] {status}: {fields}" + + @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: + return label.replace("lid_twist_turn_counterclockwise_", "반시계방향 회전 ") + if label.startswith("lid_twist_reset_clockwise_") and "_ikin" not in label: + return label.replace("lid_twist_reset_clockwise_", "되감기 시계방향 회전 ") + if label.startswith("lid_twist_reset_counterclockwise_") and "_ikin" not in label: + return label.replace("lid_twist_reset_counterclockwise_", "되감기 반시계방향 회전 ") + if label.startswith("lid_twist_turn_clockwise_") and label.endswith("_ikin"): + core = label.removesuffix("_ikin") + return core.replace("lid_twist_turn_clockwise_", "시계방향 회전 IK ") + if label.startswith("lid_twist_turn_counterclockwise_") and label.endswith("_ikin"): + core = label.removesuffix("_ikin") + return core.replace("lid_twist_turn_counterclockwise_", "반시계방향 회전 IK ") + if label.startswith("lid_twist_reset_clockwise_") and label.endswith("_ikin"): + core = label.removesuffix("_ikin") + return core.replace("lid_twist_reset_clockwise_", "되감기 시계방향 회전 IK ") + if label.startswith("lid_twist_reset_counterclockwise_") and label.endswith("_ikin"): + core = label.removesuffix("_ikin") + return core.replace("lid_twist_reset_counterclockwise_", "되감기 반시계방향 회전 IK ") + return { + "approach_lid": "1단계 접근 위치", + "grasp_lid": "2단계 파지 위치", + "lift_lid": "3단계 상승 위치", + "lid_twist_transfer_high": "4단계 고정 위치 위 안전 이동", + "lid_twist_transfer": "5단계 회전 위치 수직 하강", + "lid_twist_press": "6단계 아래로 눌러 접촉", + "lid_twist_turn_clockwise": "7단계 시계방향 회전", + "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", + "lid_twist_home_ikin": "그리퍼 open 후 안전 위치 IK", + "lid_twist_task_compliance": "힘 제어 컴플라이언스 설정", + "lid_twist_set_force": "아래 방향 목표 힘 설정", + "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": "컴플라이언스 해제", + "lid_twist_final_preopen": "회전 완료 후 그리퍼 열기", + "lid_twist_regrip_preopen": "재파지 그리퍼 열기", + "lid_twist_regrip_grasp": "재파지 그리퍼 닫기", + "gripper_preopen": "그리퍼 열기", + "gripper_grasp": "그리퍼 닫기", + "get_current_posj": "현재 관절 위치 확인", + "get_current_posx": "현재 TCP 위치 확인", + "get_last_alarm": "마지막 알람 확인", + }.get(label, label) + + @staticmethod + def _gripper_command_ko(command: str) -> str: + return { + "preopen": "열기", + "open": "열기", + "grasp": "닫기", + "close": "닫기", + "set_width": "폭 설정", + }.get(command, command) + + @staticmethod + def _fmt_coord(value: dict, key: str) -> str: + coord = value.get(key) if isinstance(value, dict) else None + return "?" if coord is None else f"{float(coord):.4f}" + + @staticmethod + def _fmt_target_mm(target: list) -> str: + if not target: + return "?" + return "[" + ", ".join(f"{float(value):.1f}" for value in target[:3]) + "]" + + @staticmethod + def _fmt_rz(target: list) -> str: + if len(target) < 6: + return "?" + return f"{float(target[5]):.1f}" + + @staticmethod + def _angle_delta_deg(actual: float, target: float) -> float: + return (float(actual) - float(target) + 180.0) % 360.0 - 180.0 + + @staticmethod + def _pose_stamped(pose, source: PoseStamped) -> PoseStamped: + msg = PoseStamped() + msg.header = source.header + msg.pose = pose + return msg + + @staticmethod + def _pose_xyz(pose) -> dict: + return { + "x": round(float(pose.position.x), 4), + "y": round(float(pose.position.y), 4), + "z": round(float(pose.position.z), 4), + } + + +def main(args=None): + rclpy.init(args=args) + node = LidGripPlannerNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() 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..e89ae17 --- /dev/null +++ b/src/azas_motion/azas_motion/link6_gripper_collision_node.py @@ -0,0 +1,319 @@ +#!/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.parameter import Parameter +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: + 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.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, + "/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() + + period = float(self.get_parameter("publish_period_sec").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() + 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 = self._operation() + + if obj.operation == CollisionObject.REMOVE: + attached.object = obj + return attached + + # 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(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, 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 + + 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, 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() + 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) 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( + 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) + 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: + 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() + + +if __name__ == "__main__": + main() 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..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 @@ -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: @@ -37,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) @@ -55,32 +60,61 @@ 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 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 + # 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 +125,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 +152,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 1d2ffaf..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,29 @@ 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", + "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", +) + +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: @@ -107,16 +130,25 @@ 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) 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) + self.declare_parameter("collision_object_exclude_ids", "") config_path = Path( 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( @@ -145,9 +177,36 @@ def __init__(self) -> None: self.publish_debug_labels = ( self.get_parameter("publish_debug_labels").get_parameter_value().bool_value ) + self.remove_legacy_collision_objects = ( + self.get_parameter("remove_legacy_collision_objects") + .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() + .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() + self._legacy_collision_objects_removed = False + self._published_ids_logged = False self._publish_scene() period = ( @@ -165,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")) @@ -206,19 +283,51 @@ 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: + 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( 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) 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] @@ -244,12 +353,26 @@ def _make_collision_object( collision_object.operation = CollisionObject.ADD return collision_object + def _make_remove_collision_object(self, object_id: str) -> CollisionObject: + collision_object = CollisionObject() + collision_object.id = object_id + collision_object.header.frame_id = self.frame_id + collision_object.operation = CollisionObject.REMOVE + return collision_object + def _make_markers( self, collision_objects: dict[str, dict[str, Any]] ) -> MarkerArray: markers: list[Marker] = [] stamp = self.get_clock().now().to_msg() + if self.clear_markers_before_publish: + clear_marker = Marker() + clear_marker.header.frame_id = self.frame_id + clear_marker.header.stamp = stamp + clear_marker.action = Marker.DELETEALL + markers.append(clear_marker) + for index, (object_id, object_config) in enumerate(collision_objects.items()): marker = Marker() marker.header.frame_id = object_config.get("frame_id", self.frame_id) @@ -346,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/side_grasp_ik_preview_node.py b/src/azas_motion/azas_motion/side_grasp_ik_preview_node.py index c4e3ac6..99392ae 100644 --- a/src/azas_motion/azas_motion/side_grasp_ik_preview_node.py +++ b/src/azas_motion/azas_motion/side_grasp_ik_preview_node.py @@ -69,6 +69,8 @@ def __init__(self) -> None: self.declare_parameter("frames_per_step", 90) self.declare_parameter("hold_frames", 35) self.declare_parameter("loop_preview", False) + self.declare_parameter("max_preview_poses", 0) + self.declare_parameter("planning_start_delay_sec", 2.5) self.declare_parameter("home_joints_rad", DEFAULT_HOME_JOINTS) self.publisher = self.create_publisher(JointState, "/joint_states", 10) @@ -79,6 +81,7 @@ def __init__(self) -> None: 10, ) self.pending_path: Optional[Path] = None + self.pending_path_time = None self.planning_started = False self.planning_failed = False self.joint_names = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] @@ -94,12 +97,13 @@ def __init__(self) -> None: ) def on_path(self, msg: Path) -> None: - if self.planning_started or self.preview_points: + if self.planning_started or self.preview_points or self.pending_path is not None: return if len(msg.poses) < 2: self.get_logger().warning("Ignoring preview Path with fewer than two poses") return self.pending_path = msg + self.pending_path_time = self.get_clock().now() self.get_logger().info(f"Received Azas dispenser sequence Path with {len(msg.poses)} poses") def on_timer(self) -> None: @@ -108,6 +112,13 @@ def on_timer(self) -> None: return self.publish_home() if self.pending_path is not None and not self.planning_started and not self.planning_failed: + delay_sec = max(float(self.get_parameter("planning_start_delay_sec").value), 0.0) + if self.pending_path_time is not None: + elapsed_sec = ( + self.get_clock().now() - self.pending_path_time + ).nanoseconds / 1_000_000_000.0 + if elapsed_sec < delay_sec: + return self.planning_started = True self.plan_preview(self.pending_path) @@ -152,7 +163,11 @@ def plan_preview(self, path: Path) -> None: try: self.ensure_moveit() points: List[List[float]] = [] - for index, pose_stamped in enumerate(path.poses, start=1): + max_preview_poses = int(self.get_parameter("max_preview_poses").value) + poses = list(path.poses) + if max_preview_poses > 0: + poses = poses[:max_preview_poses] + for index, pose_stamped in enumerate(poses, start=1): trajectory = self.plan_pose(pose_stamped) names, positions = self.trajectory_points(trajectory) if not positions: @@ -162,7 +177,7 @@ def plan_preview(self, path: Path) -> None: start = points[-1] if points else self.current_home_positions(len(target)) points.extend(self.interpolate_positions(start, target)) self.get_logger().info( - f"IK preview planned sequential pose {index}/{len(path.poses)}" + f"IK preview planned sequential pose {index}/{len(poses)}" ) self.preview_points = points self.preview_index = 0 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 a6bbced..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.43, - 0.18, - 0.392, - 0.43, - 0.08, - 0.392, - 0.43, - -0.02, - 0.392, - 0.43, - -0.12, - 0.392, + 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/azas_motion/workspace_collision_scene_node.py b/src/azas_motion/azas_motion/workspace_collision_scene_node.py new file mode 100644 index 0000000..88f10ad --- /dev/null +++ b/src/azas_motion/azas_motion/workspace_collision_scene_node.py @@ -0,0 +1,389 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import time +from pathlib import Path +from typing import Any + +import rclpy +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 +from shape_msgs.msg import SolidPrimitive + + +DEFAULT_SAFETY_CONFIG_PATH = "/home/ssu/Azas/src/azas_bringup/config/safety.yaml" + + +def transient_qos(depth: int = 10) -> QoSProfile: + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + +def parse_workspace_bounds(value: Any) -> dict[str, float] | None: + if value is None: + return None + if not isinstance(value, dict): + raise ValueError("workspace_bounds_m must be a YAML map") + + required = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max") + missing = [key for key in required if key not in value] + if missing: + raise ValueError(f"workspace_bounds_m is missing keys: {missing}") + + bounds = {key: float(value[key]) for key in required} + if bounds["x_min"] > bounds["x_max"]: + raise ValueError("workspace_bounds_m x_min must be <= x_max") + if bounds["y_min"] > bounds["y_max"]: + raise ValueError("workspace_bounds_m y_min must be <= y_max") + if bounds["z_min"] > bounds["z_max"]: + raise ValueError("workspace_bounds_m z_min must be <= z_max") + return bounds + + +class WorkspaceCollisionSceneNode(Node): + def __init__(self) -> None: + super().__init__("workspace_collision_scene_node") + + self.declare_parameter("safety_config_path", DEFAULT_SAFETY_CONFIG_PATH) + self.declare_parameter("publish_period_sec", 2.0) + self.declare_parameter("publish_collision_objects", True) + self.declare_parameter("frame_id", "base_link") + self.declare_parameter("table_collision_enabled", True) + self.declare_parameter("table_collision_id", "side_grip_table") + self.declare_parameter("table_surface_z", 0.0) + self.declare_parameter("table_thickness", 0.04) + self.declare_parameter("table_size_x", 1.20) + self.declare_parameter("table_size_y", 1.00) + self.declare_parameter("table_center_x", 0.45) + self.declare_parameter("table_center_y", 0.0) + self.declare_parameter("table_collision_expand_to_workspace_walls", True) + self.declare_parameter("publish_repeats", 3) + self.declare_parameter("workspace_boundary_collision_enabled", True) + self.declare_parameter("workspace_boundary_collision_prefix", "side_grip_workspace") + self.declare_parameter("workspace_boundary_wall_thickness", 0.04) + self.declare_parameter("workspace_boundary_wall_clearance", 0.02) + + safety_config_path = Path( + self.get_parameter("safety_config_path").get_parameter_value().string_value + ).expanduser() + self.frame_id = self.get_parameter("frame_id").get_parameter_value().string_value + self.publish_collision_objects = ( + self.get_parameter("publish_collision_objects") + .get_parameter_value() + .bool_value + ) + self.table_collision_enabled = ( + self.get_parameter("table_collision_enabled").get_parameter_value().bool_value + ) + self.table_collision_id = ( + self.get_parameter("table_collision_id").get_parameter_value().string_value + ) + self.table_surface_z = ( + self.get_parameter("table_surface_z").get_parameter_value().double_value + ) + self.table_thickness = max( + 0.001, + self.get_parameter("table_thickness").get_parameter_value().double_value, + ) + self.table_size_x = ( + self.get_parameter("table_size_x").get_parameter_value().double_value + ) + self.table_size_y = ( + self.get_parameter("table_size_y").get_parameter_value().double_value + ) + self.table_center_x = ( + self.get_parameter("table_center_x").get_parameter_value().double_value + ) + self.table_center_y = ( + self.get_parameter("table_center_y").get_parameter_value().double_value + ) + self.table_collision_expand_to_workspace_walls = ( + self.get_parameter("table_collision_expand_to_workspace_walls") + .get_parameter_value() + .bool_value + ) + self.publish_repeats = max( + 1, self.get_parameter("publish_repeats").get_parameter_value().integer_value + ) + self.workspace_boundary_collision_enabled = ( + self.get_parameter("workspace_boundary_collision_enabled") + .get_parameter_value() + .bool_value + ) + self.workspace_boundary_collision_prefix = ( + self.get_parameter("workspace_boundary_collision_prefix") + .get_parameter_value() + .string_value + ) + self.workspace_boundary_wall_thickness = max( + 0.001, + self.get_parameter("workspace_boundary_wall_thickness") + .get_parameter_value() + .double_value, + ) + self.workspace_boundary_wall_clearance = max( + 0.0, + self.get_parameter("workspace_boundary_wall_clearance") + .get_parameter_value() + .double_value, + ) + + self.workspace_bounds_m = self._load_safety_workspace_bounds(safety_config_path) + 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 + self.timer = self.create_timer(max(0.5, period), self._publish_scene) + + def _load_safety_workspace_bounds(self, safety_config_path: Path) -> dict[str, float]: + if not safety_config_path.exists(): + raise FileNotFoundError(f"safety config does not exist: {safety_config_path}") + with safety_config_path.open("r", encoding="utf-8") as stream: + safety_config = yaml.safe_load(stream) or {} + + motion_config = safety_config.get("motion", {}) + if not isinstance(motion_config, dict): + raise ValueError(f"motion section is not a YAML map: {safety_config_path}") + + bounds = parse_workspace_bounds(motion_config.get("workspace_bounds_m")) + if bounds is None: + raise ValueError( + f"motion.workspace_bounds_m must contain x/y/z min/max values: " + f"{safety_config_path}" + ) + + min_z = motion_config.get("min_z_m") + if min_z is not None and float(min_z) > bounds["z_min"]: + self.get_logger().warning( + f"safety min_z_m={float(min_z):.3f} is higher than workspace z_min=" + f"{bounds['z_min']:.3f}; using min_z_m for boundary floor height" + ) + bounds["z_min"] = float(min_z) + + self.get_logger().info( + f"Loaded workspace safety bounds from {safety_config_path}: " + f"x=[{bounds['x_min']:.3f}, {bounds['x_max']:.3f}], " + f"y=[{bounds['y_min']:.3f}, {bounds['y_max']:.3f}], " + f"z=[{bounds['z_min']:.3f}, {bounds['z_max']:.3f}]" + ) + return bounds + + def _make_box_collision_object( + self, + object_id: str, + center_xyz: list[float], + size_xyz: list[float], + ) -> CollisionObject: + collision_object = CollisionObject() + collision_object.id = object_id + collision_object.header.frame_id = self.frame_id + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = [float(value) for value in size_xyz] + + 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 _table_collision_object(self) -> CollisionObject | None: + if not self.table_collision_enabled: + return None + + table_center_x = self.table_center_x + table_center_y = self.table_center_y + table_size_x = self.table_size_x + table_size_y = self.table_size_y + if self.table_collision_expand_to_workspace_walls: + clearance = ( + self.workspace_boundary_wall_clearance + if self.workspace_boundary_collision_enabled + else 0.0 + ) + x_min = self.workspace_bounds_m["x_min"] - clearance + x_max = self.workspace_bounds_m["x_max"] + clearance + y_min = self.workspace_bounds_m["y_min"] - clearance + y_max = self.workspace_bounds_m["y_max"] + clearance + table_center_x = (x_min + x_max) * 0.5 + table_center_y = (y_min + y_max) * 0.5 + table_size_x = x_max - x_min + table_size_y = y_max - y_min + + center_z = self.table_surface_z - self.table_thickness * 0.5 + return self._make_box_collision_object( + self.table_collision_id or "side_grip_table", + [table_center_x, table_center_y, center_z], + [table_size_x, table_size_y, self.table_thickness], + ) + + def _workspace_wall_collision_objects(self) -> list[CollisionObject]: + if not self.workspace_boundary_collision_enabled: + return [] + + bounds = self.workspace_bounds_m + thickness = self.workspace_boundary_wall_thickness + clearance = self.workspace_boundary_wall_clearance + prefix = self.workspace_boundary_collision_prefix or "side_grip_workspace" + + x_min = bounds["x_min"] - clearance + x_max = bounds["x_max"] + clearance + y_min = bounds["y_min"] - clearance + y_max = bounds["y_max"] + clearance + z_min = bounds["z_min"] + z_max = bounds["z_max"] + x_span = x_max - x_min + y_span = y_max - y_min + z_span = z_max - z_min + x_mid = (x_min + x_max) * 0.5 + y_mid = (y_min + y_max) * 0.5 + z_mid = (z_min + z_max) * 0.5 + + return [ + self._make_box_collision_object( + f"{prefix}_x_min_wall", + [x_min - thickness * 0.5, y_mid, z_mid], + [thickness, y_span + 2.0 * thickness, z_span], + ), + self._make_box_collision_object( + f"{prefix}_x_max_wall", + [x_max + thickness * 0.5, y_mid, z_mid], + [thickness, y_span + 2.0 * thickness, z_span], + ), + self._make_box_collision_object( + f"{prefix}_y_min_wall", + [x_mid, y_min - thickness * 0.5, z_mid], + [x_span + 2.0 * thickness, thickness, z_span], + ), + self._make_box_collision_object( + f"{prefix}_y_max_wall", + [x_mid, y_max + thickness * 0.5, z_mid], + [x_span + 2.0 * thickness, thickness, z_span], + ), + ] + + + 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 + + objects = [] + table = self._table_collision_object() + if table is not None: + objects.append(table) + objects.extend(self._workspace_wall_collision_objects()) + + for _ in range(self.publish_repeats): + for collision_object in objects: + self.collision_pub.publish(collision_object) + time.sleep(0.05) + + table_state = "on" if table is not None else "off" + wall_state = "on" if self.workspace_boundary_collision_enabled else "off" + self.get_logger().info( + "Published workspace collision scene " + f"frame={self.frame_id}, table={table_state}, walls={wall_state}, " + f"objects={len(objects)}" + ) + + +def main() -> None: + rclpy.init() + node = WorkspaceCollisionSceneNode() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index 55e1491..e5199bc 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -32,7 +32,10 @@ "doosan_moveit_cup_target_then_shake_node = azas_motion.doosan_moveit_cup_target_then_shake_node:main", "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", + "lid_grip_planner_node = azas_motion.lid_grip_planner_node: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", "mp_waypoint_legacy = azas_motion.mp_waypoint_legacy:main", @@ -46,6 +49,7 @@ "tumbler_collision_scene_node = azas_motion.tumbler_collision_scene_node:main", "tumbler_floor_place_node = azas_motion.tumbler_floor_place_node:main", "tumbler_shake_sequence_node = azas_motion.tumbler_shake_sequence_node:main", + "workspace_collision_scene_node = azas_motion.workspace_collision_scene_node:main", ], }, ) diff --git a/src/azas_motion/test/test_alignment.py b/src/azas_motion/test/test_alignment.py index 9619042..9de5171 100644 --- a/src/azas_motion/test/test_alignment.py +++ b/src/azas_motion/test/test_alignment.py @@ -9,6 +9,7 @@ compute_observe_pose, compute_side_grasp_plan, ) +from azas_motion.lid_grip import LidGripConfig, compute_lid_grip_plan def _pose(x=0.30, y=-0.10, z=0.20): @@ -58,3 +59,76 @@ def test_observe_pose_normalizes_quaternion(): ) assert norm == pytest.approx(1.0) + +def test_lid_grip_plan_offsets_along_detected_local_z(): + pose = _pose(z=0.20) + plan = compute_lid_grip_plan( + pose, + LidGripConfig(approach_offset_m=0.08, lift_offset_m=0.10), + ) + + assert plan.grasp_pose.position.z == pytest.approx(0.20) + assert plan.approach_pose.position.z == pytest.approx(0.28) + assert plan.lift_pose.position.z == pytest.approx(0.30) + + +def test_lid_grip_plan_applies_measured_tcp_grasp_offset_in_base_frame(): + pose = _pose(x=0.30, y=-0.10, z=0.20) + plan = compute_lid_grip_plan( + pose, + LidGripConfig( + approach_offset_m=0.08, + lift_offset_m=0.10, + surface_offset_m=0.02, + offset_axis="base_z", + tcp_grasp_offset_x_m=-0.006, + tcp_grasp_offset_y_m=-0.045, + tcp_grasp_offset_z_m=-0.064, + min_grasp_z_m=0.0, + ), + ) + + assert plan.grasp_pose.position.x == pytest.approx(0.294) + assert plan.grasp_pose.position.y == pytest.approx(-0.145) + assert plan.grasp_pose.position.z == pytest.approx(0.156) + assert plan.approach_pose.position.z == pytest.approx(0.236) + assert plan.lift_pose.position.z == pytest.approx(0.256) + + +def test_lid_grip_plan_clamps_approach_height_above_low_depth_detection(): + pose = _pose(x=0.41, y=-0.06, z=0.072) + plan = compute_lid_grip_plan( + pose, + LidGripConfig( + approach_offset_m=0.08, + lift_offset_m=0.10, + min_approach_z_m=0.26, + offset_axis="base_z", + tcp_grasp_offset_z_m=0.16, + min_grasp_z_m=0.18, + ), + ) + + assert plan.grasp_pose.position.z == pytest.approx(0.232) + assert plan.approach_pose.position.z == pytest.approx(0.312) + assert plan.lift_pose.position.z == pytest.approx(0.332) + + +def test_lid_grip_plan_refuses_low_grasp_after_tcp_compensation(): + with pytest.raises(ValueError, match="LID_GRASP_Z_OUT_OF_BOUNDS"): + compute_lid_grip_plan( + _pose(z=0.01), + LidGripConfig( + offset_axis="base_z", + tcp_grasp_offset_z_m=0.16, + min_grasp_z_m=0.18, + ), + ) + + +def test_lid_grip_plan_rejects_unmeasured_z_bounds(): + with pytest.raises(ValueError, match="LID_GRASP_Z_OUT_OF_BOUNDS"): + compute_lid_grip_plan( + _pose(z=0.80), + LidGripConfig(max_grasp_z_m=0.60), + ) diff --git a/src/azas_perception/azas_perception/cup_detection_pose_bridge_node.py b/src/azas_perception/azas_perception/cup_detection_pose_bridge_node.py index 177e17f..9c91b37 100644 --- a/src/azas_perception/azas_perception/cup_detection_pose_bridge_node.py +++ b/src/azas_perception/azas_perception/cup_detection_pose_bridge_node.py @@ -1,4 +1,5 @@ import rclpy +import time from azas_interfaces.msg import CupDetection from geometry_msgs.msg import PoseStamped from rclpy.node import Node @@ -25,9 +26,14 @@ def __init__(self): self.declare_parameter("use_latest_tf_when_stamp_zero", True) self.declare_parameter("allow_latest_tf_fallback", False) self.declare_parameter("debug_pose_logging", False) + self.declare_parameter("log_published_pose", False) + self.declare_parameter("rejection_log_throttle_sec", 2.0) + self.declare_parameter("tf_fallback_log_throttle_sec", 2.0) self._tf_buffer = Buffer() self._tf_listener = TransformListener(self._tf_buffer, self) + self._last_rejection_log_by_key: dict[str, float] = {} + self._last_latest_tf_fallback_log_time = 0.0 self._pub = self.create_publisher( PoseStamped, @@ -56,21 +62,24 @@ def _on_detection(self, msg: CupDetection) -> None: # CupDetection.confidence is float32 on the wire; allow a tiny epsilon so # values displayed as equal, e.g. 0.950, are not rejected by binary # representation noise. - if msg.confidence + 1e-6 < min_confidence: - self.get_logger().warn( - f"Ignoring low confidence detection: {msg.confidence:.3f} < {min_confidence:.3f}" - ) - return if required and not msg.status.startswith(required): - self.get_logger().warn( + self._warn_rejected( + f"status:{msg.status}", f"Ignoring detection status={msg.status!r}; " - f"error_code={self._orientation_error_code(msg.status)}" + f"expected_prefix={required!r}" + ) + return + if msg.confidence + 1e-6 < min_confidence: + self._warn_rejected( + "low_confidence", + f"Ignoring low confidence detection: {msg.confidence:.3f} < {min_confidence:.3f}" ) return if bool(self.get_parameter("require_upright_status").value) and not msg.status.startswith( "detected:upright" ): - self.get_logger().warn( + self._warn_rejected( + f"non_upright:{msg.status}", "Ignoring non-upright cup detection; refusing to publish tumbler pose: " f"status={msg.status!r} error_code={self._orientation_error_code(msg.status)}" ) @@ -90,11 +99,12 @@ def _on_detection(self, msg: CupDetection) -> None: if pose_msg is None: return self._pub.publish(pose_msg) - self.get_logger().info( - "Published tumbler pose from detection: " - f"x={pose_msg.pose.position.x:.3f} y={pose_msg.pose.position.y:.3f} " - f"z={pose_msg.pose.position.z:.3f} frame={pose_msg.header.frame_id} conf={msg.confidence:.3f}" - ) + if bool(self.get_parameter("log_published_pose").value): + self.get_logger().info( + "Published tumbler pose from detection: " + f"x={pose_msg.pose.position.x:.3f} y={pose_msg.pose.position.y:.3f} " + f"z={pose_msg.pose.position.z:.3f} frame={pose_msg.header.frame_id} conf={msg.confidence:.3f}" + ) def _to_target_frame(self, pose_msg: PoseStamped): # This bridge converts camera-frame detections into base_link. If TF is @@ -133,7 +143,7 @@ def _to_target_frame(self, pose_msg: PoseStamped): rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=timeout_sec), ) - self.get_logger().warn( + self._warn_latest_tf_fallback( "Using latest TF fallback after stamped lookup failed; " "this is diagnostic-only and does not prove real-motion readiness: " f"target={target_frame} source={source_frame} " @@ -212,6 +222,29 @@ def _log_pose(self, label: str, pose_msg: PoseStamped) -> None: f"{orientation.z:.4f}, {orientation.w:.4f})" ) + def _warn_rejected(self, key: str, message: str) -> None: + throttle_sec = float(self.get_parameter("rejection_log_throttle_sec").value) + if throttle_sec <= 0.0: + self.get_logger().warn(message) + return + now = time.monotonic() + last = self._last_rejection_log_by_key.get(key) + if last is not None and now - last < throttle_sec: + return + self._last_rejection_log_by_key[key] = now + self.get_logger().warn(message) + + def _warn_latest_tf_fallback(self, message: str) -> None: + throttle_sec = float(self.get_parameter("tf_fallback_log_throttle_sec").value) + if throttle_sec <= 0.0: + self.get_logger().warn(message) + return + now = time.monotonic() + if now - self._last_latest_tf_fallback_log_time < throttle_sec: + return + self._last_latest_tf_fallback_log_time = now + self.get_logger().warn(message) + @staticmethod def _orientation_error_code(status: str) -> str: if "unknown_orientation" in status or "orientation=unknown" in status: 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/hand_eye_static_tf_node.py b/src/azas_perception/azas_perception/hand_eye_static_tf_node.py new file mode 100644 index 0000000..5be253c --- /dev/null +++ b/src/azas_perception/azas_perception/hand_eye_static_tf_node.py @@ -0,0 +1,263 @@ +from __future__ import annotations + +from pathlib import Path +import time + +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import TransformStamped +from rclpy.duration import Duration +from rclpy.node import Node +from tf2_ros import Buffer, StaticTransformBroadcaster, TransformException, TransformListener + + +def default_hand_eye_matrix_path() -> str: + return str(Path(get_package_share_directory("azas_perception")) / "config" / "T_gripper2camera.npy") + + +def normalize_quaternion(q: np.ndarray) -> np.ndarray: + norm = float(np.linalg.norm(q)) + if norm <= 0.0 or not np.isfinite(norm): + raise ValueError("rotation produced an invalid quaternion") + return q / norm + + +def quaternion_from_matrix(rotation: np.ndarray) -> np.ndarray: + matrix = np.asarray(rotation, dtype=float) + if matrix.shape != (3, 3): + raise ValueError("rotation matrix must be 3x3") + + trace = float(np.trace(matrix)) + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * s + qx = (matrix[2, 1] - matrix[1, 2]) / s + qy = (matrix[0, 2] - matrix[2, 0]) / s + qz = (matrix[1, 0] - matrix[0, 1]) / s + elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]: + s = np.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2.0 + qw = (matrix[2, 1] - matrix[1, 2]) / s + qx = 0.25 * s + qy = (matrix[0, 1] + matrix[1, 0]) / s + qz = (matrix[0, 2] + matrix[2, 0]) / s + elif matrix[1, 1] > matrix[2, 2]: + s = np.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0 + qw = (matrix[0, 2] - matrix[2, 0]) / s + qx = (matrix[0, 1] + matrix[1, 0]) / s + qy = 0.25 * s + qz = (matrix[1, 2] + matrix[2, 1]) / s + else: + s = np.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0 + qw = (matrix[1, 0] - matrix[0, 1]) / s + qx = (matrix[0, 2] + matrix[2, 0]) / s + qy = (matrix[1, 2] + matrix[2, 1]) / s + qz = 0.25 * s + + return normalize_quaternion(np.array([qx, qy, qz, qw], dtype=float)) + + +def matrix_from_quaternion(qx: float, qy: float, qz: float, qw: float) -> np.ndarray: + q = normalize_quaternion(np.array([qx, qy, qz, qw], dtype=float)) + x, y, z, w = q + return np.array( + [ + [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)], + [2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)], + [2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)], + ], + dtype=float, + ) + + +def matrix_from_transform(transform: TransformStamped) -> np.ndarray: + translation = transform.transform.translation + rotation = transform.transform.rotation + matrix = np.eye(4, dtype=float) + matrix[:3, :3] = matrix_from_quaternion(rotation.x, rotation.y, rotation.z, rotation.w) + matrix[:3, 3] = [translation.x, translation.y, translation.z] + return matrix + + +def transform_from_matrix( + matrix: np.ndarray, + parent_frame: str, + child_frame: str, + stamp, +) -> TransformStamped: + transform = TransformStamped() + transform.header.stamp = stamp + transform.header.frame_id = parent_frame + transform.child_frame_id = child_frame + transform.transform.translation.x = float(matrix[0, 3]) + transform.transform.translation.y = float(matrix[1, 3]) + transform.transform.translation.z = float(matrix[2, 3]) + qx, qy, qz, qw = quaternion_from_matrix(matrix[:3, :3]) + transform.transform.rotation.x = float(qx) + transform.transform.rotation.y = float(qy) + transform.transform.rotation.z = float(qz) + transform.transform.rotation.w = float(qw) + return transform + + +def load_hand_eye_matrix(path: str | Path, translation_scale: float) -> np.ndarray: + matrix_path = Path(path) + if not matrix_path.is_file(): + raise FileNotFoundError(f"hand-eye matrix file not found: {matrix_path}") + + matrix = np.asarray(np.load(str(matrix_path)), dtype=float) + if matrix.shape != (4, 4): + raise ValueError(f"hand-eye matrix must be 4x4, got {matrix.shape}") + if not np.all(np.isfinite(matrix)): + raise ValueError("hand-eye matrix contains non-finite values") + if not np.allclose(matrix[3, :], np.array([0.0, 0.0, 0.0, 1.0]), atol=1e-6): + raise ValueError("hand-eye matrix bottom row must be [0, 0, 0, 1]") + if not np.isfinite(translation_scale) or translation_scale <= 0.0: + raise ValueError("translation_scale must be a positive finite value") + + scaled = matrix.copy() + scaled[:3, 3] *= float(translation_scale) + rotation = scaled[:3, :3] + orthogonality_error = float(np.linalg.norm(rotation.T @ rotation - np.eye(3))) + determinant = float(np.linalg.det(rotation)) + if orthogonality_error > 1e-3: + raise ValueError(f"hand-eye rotation is not orthonormal: error={orthogonality_error:.6g}") + if abs(determinant - 1.0) > 1e-3: + raise ValueError(f"hand-eye rotation determinant must be near +1, got {determinant:.6g}") + return scaled + + +def compose_parent_to_published_child( + parent_from_matrix_child: np.ndarray, + published_child_from_matrix_child: np.ndarray, +) -> np.ndarray: + return parent_from_matrix_child @ np.linalg.inv(published_child_from_matrix_child) + + +class HandEyeStaticTfNode(Node): + """Publish measured hand-eye calibration as a TF tree connection. + + The bundled legacy calibration matrix is expressed in the camera optical + frame. RealSense usually already owns camera_link -> camera_color_optical_frame, + so this node composes the measured matrix with that existing camera TF and + publishes link_6 -> camera_link. That connects the camera tree to the robot + tree without assigning a second parent to camera_color_optical_frame. + """ + + def __init__(self): + super().__init__("hand_eye_static_tf_node") + self.declare_parameter("matrix_path", "") + self.declare_parameter("parent_frame", "link_6") + self.declare_parameter("matrix_child_frame", "camera_color_optical_frame") + self.declare_parameter("published_child_frame", "camera_link") + self.declare_parameter("translation_scale", 0.001) + self.declare_parameter("compose_with_existing_tf", True) + self.declare_parameter("compose_timeout_sec", 5.0) + self.declare_parameter("allow_direct_fallback", False) + + self._broadcaster = StaticTransformBroadcaster(self) + self._tf_buffer = Buffer() + self._tf_listener = TransformListener(self._tf_buffer, self) + self._started_at = time.monotonic() + self._published = False + + matrix_path = str(self.get_parameter("matrix_path").value).strip() or default_hand_eye_matrix_path() + translation_scale = float(self.get_parameter("translation_scale").value) + try: + self._parent_from_matrix_child = load_hand_eye_matrix(matrix_path, translation_scale) + except Exception as exc: + self.get_logger().error(f"Cannot publish hand-eye TF: {exc}") + self._timer = None + return + + self._matrix_path = matrix_path + self._timer = self.create_timer(0.1, self._try_publish) + self.get_logger().info( + "Loaded hand-eye matrix: " + f"path={matrix_path} parent={self.get_parameter('parent_frame').value} " + f"matrix_child={self.get_parameter('matrix_child_frame').value} " + f"published_child={self.get_parameter('published_child_frame').value} " + f"translation_scale={translation_scale}" + ) + + def _try_publish(self) -> None: + if self._published: + return + + parent_frame = str(self.get_parameter("parent_frame").value).strip() + matrix_child_frame = str(self.get_parameter("matrix_child_frame").value).strip() + published_child_frame = str(self.get_parameter("published_child_frame").value).strip() + if not parent_frame or not matrix_child_frame or not published_child_frame: + self.get_logger().error("parent_frame, matrix_child_frame, and published_child_frame are required") + self._stop_timer() + return + + transform_matrix = self._parent_from_matrix_child + compose = ( + bool(self.get_parameter("compose_with_existing_tf").value) + and published_child_frame != matrix_child_frame + ) + if compose: + try: + camera_transform = self._tf_buffer.lookup_transform( + published_child_frame, + matrix_child_frame, + rclpy.time.Time(), + timeout=Duration(seconds=0.05), + ) + transform_matrix = compose_parent_to_published_child( + self._parent_from_matrix_child, + matrix_from_transform(camera_transform), + ) + except TransformException as exc: + timeout_sec = float(self.get_parameter("compose_timeout_sec").value) + if time.monotonic() - self._started_at < timeout_sec: + return + if bool(self.get_parameter("allow_direct_fallback").value): + self.get_logger().warn( + "Falling back to direct hand-eye TF because camera frame composition failed: " + f"{published_child_frame}->{matrix_child_frame}: {exc}" + ) + published_child_frame = matrix_child_frame + transform_matrix = self._parent_from_matrix_child + else: + self.get_logger().error( + "Cannot compose hand-eye TF with existing camera tree; " + f"missing TF target={published_child_frame} source={matrix_child_frame}: {exc}" + ) + self._stop_timer() + return + + transform = transform_from_matrix( + transform_matrix, + parent_frame, + published_child_frame, + self.get_clock().now().to_msg(), + ) + self._broadcaster.sendTransform(transform) + self._published = True + self._stop_timer() + translation = transform.transform.translation + rotation = transform.transform.rotation + self.get_logger().info( + "Published measured hand-eye static TF: " + f"{parent_frame} -> {published_child_frame} " + f"xyz=({translation.x:.6f}, {translation.y:.6f}, {translation.z:.6f}) " + f"quat=({rotation.x:.6f}, {rotation.y:.6f}, {rotation.z:.6f}, {rotation.w:.6f}) " + f"source_matrix={self._matrix_path}" + ) + + def _stop_timer(self) -> None: + if self._timer is not None: + self._timer.cancel() + + +def main(args=None): + rclpy.init(args=args) + node = HandEyeStaticTfNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/azas_perception/azas_perception/lid_marker.py b/src/azas_perception/azas_perception/lid_marker.py new file mode 100644 index 0000000..792dfec --- /dev/null +++ b/src/azas_perception/azas_perception/lid_marker.py @@ -0,0 +1,398 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass + +import cv2 +import numpy as np + + +@dataclass(frozen=True) +class ImageRoi: + x_min: int + y_min: int + x_max: int + y_max: int + + @property + def width(self) -> int: + return max(self.x_max - self.x_min, 0) + + @property + def height(self) -> int: + return max(self.y_max - self.y_min, 0) + + +@dataclass(frozen=True) +class RedCircleConfig: + min_area_px: float = 80.0 + min_radius_px: float = 4.0 + min_circularity: float = 0.65 + min_saturation: int = 80 + min_value: int = 40 + morph_kernel_px: int = 3 + + +@dataclass(frozen=True) +class RedCircle: + center_u: int + center_v: int + radius_px: float + area_px: float + circularity: float + + +@dataclass(frozen=True) +class ArucoMarker: + center_u: int + center_v: int + marker_id: int + side_px: float + corners: np.ndarray + + +def padded_roi(roi: ImageRoi, image_width: int, image_height: int, padding_ratio: float) -> ImageRoi: + padding_ratio = max(float(padding_ratio), 0.0) + pad_x = int(round(roi.width * padding_ratio)) + pad_y = int(round(roi.height * padding_ratio)) + return ImageRoi( + x_min=max(roi.x_min - pad_x, 0), + y_min=max(roi.y_min - pad_y, 0), + x_max=min(roi.x_max + pad_x, image_width), + y_max=min(roi.y_max + pad_y, image_height), + ) + + +def detect_red_circle_marker( + image_bgr: np.ndarray, + roi: ImageRoi, + config: RedCircleConfig, +) -> RedCircle | None: + if image_bgr.size == 0 or roi.width <= 0 or roi.height <= 0: + return None + + patch = image_bgr[roi.y_min : roi.y_max, roi.x_min : roi.x_max] + if patch.size == 0: + return None + + hsv = cv2.cvtColor(patch, cv2.COLOR_BGR2HSV) + lower_red_a = np.array([0, config.min_saturation, config.min_value], dtype=np.uint8) + upper_red_a = np.array([10, 255, 255], dtype=np.uint8) + lower_red_b = np.array([170, config.min_saturation, config.min_value], dtype=np.uint8) + upper_red_b = np.array([180, 255, 255], dtype=np.uint8) + mask = cv2.inRange(hsv, lower_red_a, upper_red_a) | cv2.inRange(hsv, lower_red_b, upper_red_b) + + kernel_px = max(int(config.morph_kernel_px), 1) + if kernel_px > 1: + kernel = np.ones((kernel_px, kernel_px), dtype=np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) + + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best: RedCircle | None = None + best_score = -1.0 + for contour in contours: + area = float(cv2.contourArea(contour)) + if area < config.min_area_px: + continue + perimeter = float(cv2.arcLength(contour, True)) + if perimeter <= 0.0: + continue + circularity = float(4.0 * math.pi * area / (perimeter * perimeter)) + if circularity < config.min_circularity: + continue + (local_x, local_y), radius = cv2.minEnclosingCircle(contour) + if radius < config.min_radius_px: + continue + moments = cv2.moments(contour) + if abs(moments["m00"]) > 1e-9: + local_u = moments["m10"] / moments["m00"] + local_v = moments["m01"] / moments["m00"] + else: + local_u = local_x + local_v = local_y + candidate = RedCircle( + center_u=int(round(roi.x_min + local_u)), + center_v=int(round(roi.y_min + local_v)), + radius_px=float(radius), + area_px=area, + circularity=circularity, + ) + score = area * circularity + if score > best_score: + best = candidate + best_score = score + return best + + +def detect_aruco_marker( + image_bgr: np.ndarray, + roi: ImageRoi, + dictionary_name: str = "DICT_4X4_50", + marker_id: int = -1, +) -> ArucoMarker | None: + if image_bgr.size == 0 or roi.width <= 0 or roi.height <= 0: + return None + + patch = image_bgr[roi.y_min : roi.y_max, roi.x_min : roi.x_max] + if patch.size == 0: + return None + + dictionary_id = _aruco_dictionary_id(dictionary_name) + if dictionary_id is None: + return None + + gray = cv2.cvtColor(patch, cv2.COLOR_BGR2GRAY) + dictionary = _create_aruco_dictionary(dictionary_id) + 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, 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: + aruco = getattr(cv2, "aruco", None) + if aruco is None: + return None + name = str(dictionary_name).strip().upper() + if not name: + return None + if not name.startswith("DICT_"): + name = f"DICT_{name}" + return getattr(aruco, name, None) + + +def _create_aruco_dictionary(dictionary_id: int): + aruco = getattr(cv2, "aruco", None) + if aruco is None: + return None + if hasattr(aruco, "getPredefinedDictionary"): + return aruco.getPredefinedDictionary(dictionary_id) + if hasattr(aruco, "Dictionary_get"): + return aruco.Dictionary_get(dictionary_id) + return None + + +def _create_aruco_detector_parameters(): + aruco = getattr(cv2, "aruco", None) + if aruco is None: + return None + if hasattr(aruco, "DetectorParameters"): + parameters = aruco.DetectorParameters() + elif hasattr(aruco, "DetectorParameters_create"): + parameters = aruco.DetectorParameters_create() + else: + return None + return _tune_lid_aruco_detector_parameters(parameters) + + +def _tune_lid_aruco_detector_parameters(parameters): + 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. + if parameters is None: + return None + 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(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): + aruco = getattr(cv2, "aruco", None) + if aruco is None: + return [], None, [] + if hasattr(aruco, "ArucoDetector"): + detector = aruco.ArucoDetector(dictionary, parameters) + return detector.detectMarkers(gray) + if hasattr(aruco, "detectMarkers"): + kwargs = {"parameters": parameters} if parameters is not None else {} + return aruco.detectMarkers(gray, dictionary, **kwargs) + return [], None, [] + + +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 + + 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) * 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: + continue + center = np.mean(global_corners, axis=0) + candidate = ArucoMarker( + center_u=int(round(float(center[0]))), + center_v=int(round(float(center[1]))), + marker_id=detected_id, + side_px=side_px, + corners=global_corners, + ) + if side_px > best_score: + best = candidate + best_score = side_px + return best + + +def _aruco_side_px(corners: np.ndarray) -> float: + if corners.shape != (4, 2): + return 0.0 + lengths = [] + for index in range(4): + current = corners[index] + next_corner = corners[(index + 1) % 4] + lengths.append(float(np.linalg.norm(next_corner - current))) + return float(np.mean(lengths)) + + +def quaternion_from_lid_normal(normal: np.ndarray, finger_axis_hint: np.ndarray | None = None) -> tuple[float, float, float, float]: + """Build a pose quaternion whose local +Z points away from the lid surface. + + The red sticker is circular, so it does not define in-plane yaw. The local + +X axis uses a projected camera-axis hint as a deterministic finger-axis + candidate; downstream motion must still verify actual gripper clearance. + """ + z_axis = _unit_vector(np.asarray(normal, dtype=float)) + if z_axis is None: + raise ValueError("lid normal must be non-zero") + + if finger_axis_hint is None: + finger_axis_hint = np.array([1.0, 0.0, 0.0], dtype=float) + x_hint = np.asarray(finger_axis_hint, dtype=float) + x_axis = x_hint - float(np.dot(x_hint, z_axis)) * z_axis + x_axis = _unit_vector(x_axis) + if x_axis is None: + fallback = np.array([0.0, 1.0, 0.0], dtype=float) + x_axis = _unit_vector(fallback - float(np.dot(fallback, z_axis)) * z_axis) + if x_axis is None: + raise ValueError("could not construct lid finger axis") + + y_axis = _unit_vector(np.cross(z_axis, x_axis)) + if y_axis is None: + raise ValueError("could not construct lid orientation basis") + x_axis = _unit_vector(np.cross(y_axis, z_axis)) + rotation = np.column_stack((x_axis, y_axis, z_axis)) + return quaternion_from_rotation_matrix(rotation) + + +def quaternion_from_rotation_matrix(rotation: np.ndarray) -> tuple[float, float, float, float]: + matrix = np.asarray(rotation, dtype=float) + if matrix.shape != (3, 3): + raise ValueError("rotation matrix must be 3x3") + + trace = float(np.trace(matrix)) + if trace > 0.0: + scale = math.sqrt(trace + 1.0) * 2.0 + qw = 0.25 * scale + qx = (matrix[2, 1] - matrix[1, 2]) / scale + qy = (matrix[0, 2] - matrix[2, 0]) / scale + qz = (matrix[1, 0] - matrix[0, 1]) / scale + elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]: + scale = math.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2.0 + qw = (matrix[2, 1] - matrix[1, 2]) / scale + qx = 0.25 * scale + qy = (matrix[0, 1] + matrix[1, 0]) / scale + qz = (matrix[0, 2] + matrix[2, 0]) / scale + elif matrix[1, 1] > matrix[2, 2]: + scale = math.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0 + qw = (matrix[0, 2] - matrix[2, 0]) / scale + qx = (matrix[0, 1] + matrix[1, 0]) / scale + qy = 0.25 * scale + qz = (matrix[1, 2] + matrix[2, 1]) / scale + else: + scale = math.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0 + qw = (matrix[1, 0] - matrix[0, 1]) / scale + qx = (matrix[0, 2] + matrix[2, 0]) / scale + qy = (matrix[1, 2] + matrix[2, 1]) / scale + qz = 0.25 * scale + + norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) + if norm == 0.0: + raise ValueError("rotation produced a zero quaternion") + return qx / norm, qy / norm, qz / norm, qw / norm + + +def _unit_vector(value: np.ndarray) -> np.ndarray | None: + norm = float(np.linalg.norm(value)) + if not math.isfinite(norm) or norm <= 1e-12: + return None + return value / norm diff --git a/src/azas_perception/azas_perception/lid_sticker_detector_node.py b/src/azas_perception/azas_perception/lid_sticker_detector_node.py new file mode 100644 index 0000000..d5a07e4 --- /dev/null +++ b/src/azas_perception/azas_perception/lid_sticker_detector_node.py @@ -0,0 +1,1012 @@ +from __future__ import annotations + +from dataclasses import dataclass +import json +import math +import os +from pathlib import Path +import time +from typing import Optional + +import cv2 +import numpy as np +import rclpy +from azas_interfaces.msg import CupDetection +from geometry_msgs.msg import Pose +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from std_msgs.msg import String + +from azas_perception.depth_projection import CameraIntrinsics, pixel_depth_to_camera_point +from azas_perception.lid_marker import ( + ArucoMarker, + ImageRoi, + RedCircle, + RedCircleConfig, + detect_aruco_marker, + detect_red_circle_marker, + padded_roi, + quaternion_from_lid_normal, +) +from azas_perception.yolo_tumbler_detector_node import DepthSample, YoloTumblerDetectorNode + +try: + from ultralytics import YOLO +except ImportError: # pragma: no cover - depends on deployment environment + YOLO = None + + +@dataclass(frozen=True) +class LidDetection2D: + roi: ImageRoi + center_u: int + center_v: int + area: int + confidence: float + class_name: str + + +@dataclass(frozen=True) +class PlaneEstimate: + normal: np.ndarray + point_count: int + rmse_m: float + + +def _default_model_path() -> str: + env_path = os.environ.get("AZAS_YOLO_MODEL_PATH") or os.environ.get("MODEL_PATH") + candidates = [ + env_path, + "/home/ssu/Downloads/best.pt", + str( + Path(__file__).resolve().parents[3] + / "src" + / "cocktail_robot_system" + / "models" + / "best.pt" + ), + "/home/ssu/ros2_ws/src/cocktail_robot_system/src/cocktail_robot_system/models/best.pt", + ] + for candidate in candidates: + if candidate and Path(candidate).is_file(): + return str(candidate) + return "/home/ssu/Downloads/best.pt" + + +class LidStickerDetectorNode(Node): + """Detect a trained lid and refine its grip pose from a center marker. + + This node publishes a camera-frame detection only. It never commands the + robot or gripper. An ArUco or red marker supplies the lid center; a local + depth plane around that center supplies the grip approach angle. + """ + + def __init__(self): + super().__init__("lid_sticker_detector_node") + self.declare_parameter("model_path", _default_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") + self.declare_parameter("output_topic", "/azas/lid_detection") + self.declare_parameter("grip_request_topic", "/jarvis/lid_gripper/grip_request") + self.declare_parameter("confidence_threshold", 0.35) + self.declare_parameter("target_class_names", "lid") + self.declare_parameter("selection_policy", "highest_confidence") + self.declare_parameter("device", "cpu") + self.declare_parameter("source_frame", "camera_color_optical_frame") + self.declare_parameter("depth_window_size", 7) + self.declare_parameter("depth_scale_mode", "auto") + self.declare_parameter("depth_scale", 0.001) + self.declare_parameter("min_depth_m", 0.15) + self.declare_parameter("max_depth_m", 2.0) + self.declare_parameter("source", "lid_aruco_detector") + self.declare_parameter("marker_type", "aruco") + self.declare_parameter("require_lid_detection", True) + self.declare_parameter("allow_aruco_only_after_grip_request", True) + self.declare_parameter("aruco_only_after_grip_request_sec", 20.0) + self.declare_parameter("roi_padding_ratio", 0.12) + self.declare_parameter("require_red_marker", True) + self.declare_parameter("red_min_area_px", 80.0) + self.declare_parameter("red_min_radius_px", 4.0) + self.declare_parameter("red_min_circularity", 0.65) + 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_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) + self.declare_parameter("require_plane_normal", True) + self.declare_parameter("plane_patch_radius_px", 18) + self.declare_parameter("plane_sample_stride_px", 2) + self.declare_parameter("min_plane_points", 20) + self.declare_parameter("max_plane_rmse_m", 0.015) + self.declare_parameter("log_detections", False) + 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 = "" + self._latest_depth_error = "" + self._latest_info: Optional[CameraInfo] = None + self._last_depth_scale_log = "" + 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() + + self._pub = self.create_publisher( + CupDetection, + str(self.get_parameter("output_topic").value), + 10, + ) + self._grip_request_pub = self.create_publisher( + String, + str(self.get_parameter("grip_request_topic").value), + 10, + ) + self.create_subscription(Image, self.get_parameter("color_topic").value, self._on_color, 10) + self.create_subscription(Image, self.get_parameter("depth_topic").value, self._on_depth, 10) + self.create_subscription( + CameraInfo, + self.get_parameter("camera_info_topic").value, + self._on_camera_info, + 10, + ) + self.get_logger().info("Lid sticker detector ready") + + def _load_model(self): + if not bool(self.get_parameter("require_lid_detection").value): + return None + model_path = str(self.get_parameter("model_path").value) + if YOLO is None: + self.get_logger().error( + "ultralytics is not installed; install it before live lid detection" + ) + return None + try: + model = YOLO(model_path) + except Exception as exc: + self.get_logger().error(f"failed to load YOLO model {model_path}: {exc}") + return None + self.get_logger().info(f"loaded YOLO lid model: {model_path}") + return model + + def _on_depth(self, msg: Image) -> None: + encoding = msg.encoding.lower() + if encoding not in YoloTumblerDetectorNode._auto_depth_scales(): + self._latest_depth = None + self._latest_depth_encoding = encoding + self._latest_depth_error = f"unsupported_depth_encoding:{msg.encoding}" + self.get_logger().error( + "Rejecting depth image with unsupported encoding " + f"{msg.encoding}; expected 16UC1, mono16, or 32FC1" + ) + return + try: + self._latest_depth = YoloTumblerDetectorNode._image_to_array(msg) + self._latest_depth_encoding = encoding + self._latest_depth_error = "" + except Exception as exc: + self._latest_depth = None + self._latest_depth_encoding = encoding + self._latest_depth_error = "depth_conversion_failed" + self.get_logger().error(f"depth conversion failed: {exc}") + + def _on_camera_info(self, msg: CameraInfo) -> None: + self._latest_info = msg + + def _on_color(self, msg: Image) -> None: + try: + image = YoloTumblerDetectorNode._image_to_bgr(msg) + except Exception as exc: + self.get_logger().error(f"color conversion failed: {exc}") + self._publish_invalid(msg, "color_conversion_failed") + return + + if self._latest_depth_error: + self._publish_invalid(msg, self._latest_depth_error) + self._show_preview(image, self._latest_depth_error) + return + if self._latest_depth is None or self._latest_info is None: + self._publish_invalid(msg, "waiting_for_depth_and_camera_info") + self._show_preview(image, "waiting_for_depth_and_camera_info") + return + + lid = self._detect_lid_if_required(image, msg) + if lid is False: + return + + marker_roi = self._marker_roi(image, lid) + marker = self._detect_marker(image, marker_roi) + if marker is None: + status = "no_aruco_marker" if self._marker_type() == "aruco" else "no_red_sticker_marker" + self._publish_invalid(msg, status) + self._show_preview(image, status, lid=lid if isinstance(lid, LidDetection2D) else None) + return + + depth = self._median_depth(marker.center_u, marker.center_v) + if depth is None: + self._publish_invalid(msg, "invalid_depth_at_marker") + self._show_preview( + image, + "invalid_depth_at_marker", + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + ) + return + + info = self._latest_info + intrinsics = CameraIntrinsics(fx=info.k[0], fy=info.k[4], cx=info.k[2], cy=info.k[5]) + if not YoloTumblerDetectorNode._valid_intrinsics(intrinsics): + self.get_logger().error( + "Invalid CameraInfo intrinsics; refusing lid projection: " + f"fx={intrinsics.fx} fy={intrinsics.fy} cx={intrinsics.cx} cy={intrinsics.cy}" + ) + self._publish_invalid(msg, "invalid_camera_info") + self._show_preview( + image, + "invalid_camera_info", + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + ) + return + + try: + x, y, z = pixel_depth_to_camera_point( + marker.center_u, + marker.center_v, + float(depth.raw), + intrinsics, + depth_scale=depth.scale, + ) + except ValueError as exc: + self.get_logger().error(f"lid marker depth projection failed: {exc}") + self._publish_invalid(msg, "invalid_projected_depth") + self._show_preview( + image, + "invalid_projected_depth", + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + ) + return + + plane = self._estimate_plane(marker.center_u, marker.center_v, intrinsics, depth.scale) + if plane is None: + if bool(self.get_parameter("require_plane_normal").value): + self._publish_invalid(msg, "invalid_lid_plane") + self._show_preview( + image, + "invalid_lid_plane", + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + depth_m=depth.meters, + ) + return + normal = np.array([0.0, 0.0, -1.0], dtype=float) + plane_points = 0 + plane_rmse = math.nan + else: + normal = plane.normal + plane_points = plane.point_count + plane_rmse = plane.rmse_m + + finger_axis_hint = self._finger_axis_hint(marker, intrinsics) + try: + qx, qy, qz, qw = quaternion_from_lid_normal(normal, finger_axis_hint) + except ValueError as exc: + self.get_logger().error(f"lid orientation construction failed: {exc}") + self._publish_invalid(msg, "invalid_lid_orientation") + self._show_preview( + image, + "invalid_lid_orientation", + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + depth_m=depth.meters, + ) + return + + lid_confidence = float(lid.confidence) if isinstance(lid, LidDetection2D) else 1.0 + output = CupDetection() + output.header.stamp = msg.header.stamp + output.header.frame_id = self._source_frame(info, msg) + output.grasp_pose = self._pose_at(x, y, z, qx, qy, qz, qw) + output.cup_mouth_center = output.grasp_pose + output.confidence = lid_confidence + output.status = self._detected_status( + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + depth=depth, + normal=normal, + plane_points=plane_points, + plane_rmse=plane_rmse, + finger_axis_hint=finger_axis_hint, + ) + output.source = str(self.get_parameter("source").value) + 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: " + f"frame={output.header.frame_id} x={x:.4f} y={y:.4f} z={z:.4f} " + f"normal=({normal[0]:.3f},{normal[1]:.3f},{normal[2]:.3f}) " + f"conf={lid_confidence:.3f}" + ) + self._show_preview( + image, + output.status, + lid=lid if isinstance(lid, LidDetection2D) else None, + marker=marker, + depth_m=depth.meters, + valid=True, + ) + + def _detect_lid_if_required(self, image: np.ndarray, msg: Image) -> LidDetection2D | bool | None: + require_lid = bool(self.get_parameter("require_lid_detection").value) + if self._model is None: + if require_lid: + self._publish_invalid(msg, "model_not_loaded") + self._show_preview(image, "model_not_loaded") + return False + return None + try: + lid = self._detect_best_lid(image) + except Exception as exc: + self.get_logger().error(f"YOLO lid prediction failed: {exc}") + if require_lid: + self._publish_invalid(msg, "prediction_failed") + self._show_preview(image, "prediction_failed") + return False + return None + if lid is None and require_lid: + if self._aruco_only_after_grip_request_active(): + return None + self._publish_invalid(msg, "no_lid_detection") + self._show_preview(image, "no_lid_detection") + return False + return lid + + def _marker_roi(self, image: np.ndarray, lid: LidDetection2D | bool | None) -> ImageRoi: + if isinstance(lid, LidDetection2D): + return padded_roi( + lid.roi, + image_width=image.shape[1], + image_height=image.shape[0], + padding_ratio=float(self.get_parameter("roi_padding_ratio").value), + ) + return ImageRoi(0, 0, image.shape[1], image.shape[0]) + + def _detect_marker(self, image: np.ndarray, marker_roi: ImageRoi) -> ArucoMarker | RedCircle | None: + marker_type = self._marker_type() + if marker_type == "aruco": + 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: + return marker + if not bool(self.get_parameter("require_red_marker").value): + return RedCircle( + center_u=int(round((marker_roi.x_min + marker_roi.x_max) / 2.0)), + center_v=int(round((marker_roi.y_min + marker_roi.y_max) / 2.0)), + radius_px=0.0, + area_px=0.0, + circularity=0.0, + ) + 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, + intrinsics: CameraIntrinsics, + ) -> np.ndarray | None: + if not isinstance(marker, ArucoMarker): + return None + if not bool(self.get_parameter("use_aruco_axis_for_orientation").value): + return None + corners = np.asarray(marker.corners, dtype=float) + if corners.shape != (4, 2): + return None + quarter_turns = int(self.get_parameter("aruco_finger_axis_quarter_turns").value) % 4 + start = corners[quarter_turns] + end = corners[(quarter_turns + 1) % 4] + delta = end - start + if float(np.linalg.norm(delta)) <= 1e-9: + return None + return np.array( + [ + float(delta[0]) / float(intrinsics.fx), + float(delta[1]) / float(intrinsics.fy), + 0.0, + ], + dtype=float, + ) + + def _detected_status( + self, + lid: LidDetection2D | None, + marker: ArucoMarker | RedCircle, + depth: DepthSample, + normal: np.ndarray, + plane_points: int, + plane_rmse: float, + finger_axis_hint: np.ndarray | None = None, + ) -> str: + parts = ["detected:lid"] + if lid is not None: + parts.extend( + [ + f"class={lid.class_name}", + f"bbox={lid.roi.width}x{lid.roi.height}", + f"lid_center=({lid.center_u},{lid.center_v})", + ] + ) + else: + parts.extend(["class=aruco_marker", "bbox=none"]) + if isinstance(marker, ArucoMarker): + parts.extend( + [ + "marker_type=aruco", + f"aruco_id={marker.marker_id}", + f"aruco_center=({marker.center_u},{marker.center_v})", + f"aruco_side_px={marker.side_px:.1f}", + f"aruco_marker_length_m={float(self.get_parameter('aruco_marker_length_m').value):.3f}", + ] + ) + else: + parts.extend( + [ + "marker_type=red", + f"red_center=({marker.center_u},{marker.center_v})", + f"red_radius_px={marker.radius_px:.1f}", + f"red_circularity={marker.circularity:.3f}", + ] + ) + parts.extend( + [ + f"depth_raw={depth.raw:.1f}", + f"depth_m={depth.meters:.3f}", + f"depth_encoding={depth.encoding}", + f"depth_scale={depth.scale:.6g}", + f"normal=({normal[0]:.4f},{normal[1]:.4f},{normal[2]:.4f})", + f"plane_points={plane_points}", + f"plane_rmse_m={plane_rmse:.4f}", + ] + ) + if isinstance(marker, ArucoMarker) and finger_axis_hint is not None: + parts.extend( + [ + f"aruco_axis_quarter_turns={int(self.get_parameter('aruco_finger_axis_quarter_turns').value)}", + ( + "aruco_axis_hint=" + f"({finger_axis_hint[0]:.4f},{finger_axis_hint[1]:.4f},{finger_axis_hint[2]:.4f})" + ), + ] + ) + return " ".join(parts) + + def _detect_best_lid(self, image: np.ndarray) -> LidDetection2D | None: + threshold = float(self.get_parameter("confidence_threshold").value) + target_names = self._target_class_names() + selection_policy = str(self.get_parameter("selection_policy").value).strip().lower() + device = str(self.get_parameter("device").value).strip() or "cpu" + results = self._model.predict(image, verbose=False, device=device) + if not results: + return None + + names = getattr(results[0], "names", {}) or {} + best = None + for box in results[0].boxes: + confidence = float(box.conf[0]) + if confidence < threshold: + continue + class_id = int(box.cls[0]) + class_name = str(names.get(class_id, class_id)).lower() + if target_names and not any(target in class_name for target in target_names): + continue + x1, y1, x2, y2 = [int(round(v)) for v in box.xyxy[0].tolist()] + x1 = max(min(x1, image.shape[1]), 0) + x2 = max(min(x2, image.shape[1]), 0) + y1 = max(min(y1, image.shape[0]), 0) + y2 = max(min(y2, image.shape[0]), 0) + width = max(x2 - x1, 0) + height = max(y2 - y1, 0) + if width <= 0 or height <= 0: + continue + candidate = LidDetection2D( + roi=ImageRoi(x1, y1, x2, y2), + center_u=int(round((x1 + x2) / 2.0)), + center_v=int(round((y1 + y2) / 2.0)), + area=width * height, + confidence=confidence, + class_name=class_name, + ) + if self._is_better_detection(candidate, best, selection_policy): + best = candidate + return best + + def _median_depth(self, u: int, v: int) -> DepthSample | None: + depth = self._latest_depth + if depth is None or depth.size == 0: + return None + window_size = self._depth_window_size() + radius = window_size // 2 + height, width = depth.shape[:2] + x1, x2 = max(u - radius, 0), min(u + radius + 1, width) + y1, y2 = max(v - radius, 0), min(v + radius + 1, height) + patch = np.asarray(depth[y1:y2, x1:x2], dtype=np.float32) + total_count = int(patch.size) + finite_positive = patch[np.isfinite(patch) & (patch > 0)] + if finite_positive.size == 0: + return None + depth_scale = self._depth_scale() + if depth_scale is None: + return None + min_depth_m = float(self.get_parameter("min_depth_m").value) + max_depth_m = float(self.get_parameter("max_depth_m").value) + self._log_depth_scale(depth_scale) + depth_m = finite_positive * depth_scale + valid = finite_positive[ + np.isfinite(depth_m) + & (depth_m >= min_depth_m) + & (depth_m <= max_depth_m) + ] + if valid.size == 0: + return None + median_raw = float(np.median(valid)) + return DepthSample( + raw=median_raw, + meters=median_raw * depth_scale, + scale=depth_scale, + encoding=self._latest_depth_encoding, + valid_count=int(valid.size), + total_count=total_count, + window_size=window_size, + ) + + def _estimate_plane( + self, + center_u: int, + center_v: int, + intrinsics: CameraIntrinsics, + depth_scale: float, + ) -> PlaneEstimate | None: + depth = self._latest_depth + if depth is None or depth.size == 0: + return None + radius = max(int(self.get_parameter("plane_patch_radius_px").value), 1) + stride = max(int(self.get_parameter("plane_sample_stride_px").value), 1) + min_depth_m = float(self.get_parameter("min_depth_m").value) + max_depth_m = float(self.get_parameter("max_depth_m").value) + height, width = depth.shape[:2] + points = [] + for v in range(max(center_v - radius, 0), min(center_v + radius + 1, height), stride): + for u in range(max(center_u - radius, 0), min(center_u + radius + 1, width), stride): + if (u - center_u) * (u - center_u) + (v - center_v) * (v - center_v) > radius * radius: + continue + raw = float(depth[v, u]) + if not math.isfinite(raw) or raw <= 0.0: + continue + depth_m = raw * depth_scale + if depth_m < min_depth_m or depth_m > max_depth_m: + continue + try: + points.append( + pixel_depth_to_camera_point(u, v, raw, intrinsics, depth_scale=depth_scale) + ) + except ValueError: + continue + min_points = int(self.get_parameter("min_plane_points").value) + if len(points) < min_points: + self.get_logger().warn( + f"Not enough valid lid plane points: {len(points)} < {min_points}" + ) + return None + point_array = np.asarray(points, dtype=float) + centroid = np.mean(point_array, axis=0) + centered = point_array - centroid + try: + _, _, vh = np.linalg.svd(centered, full_matrices=False) + except np.linalg.LinAlgError as exc: + self.get_logger().warn(f"lid plane SVD failed: {exc}") + return None + normal = vh[-1] + norm = float(np.linalg.norm(normal)) + if norm <= 1e-12: + return None + normal = normal / norm + if normal[2] > 0.0: + normal = -normal + residuals = centered @ normal + rmse = float(math.sqrt(float(np.mean(residuals * residuals)))) + max_rmse = float(self.get_parameter("max_plane_rmse_m").value) + if max_rmse > 0.0 and rmse > max_rmse: + self.get_logger().warn( + f"Lid plane RMSE too high: {rmse:.4f}m > {max_rmse:.4f}m" + ) + return None + return PlaneEstimate(normal=normal, point_count=len(points), rmse_m=rmse) + + def _red_circle_config(self) -> RedCircleConfig: + return RedCircleConfig( + min_area_px=float(self.get_parameter("red_min_area_px").value), + min_radius_px=float(self.get_parameter("red_min_radius_px").value), + min_circularity=float(self.get_parameter("red_min_circularity").value), + min_saturation=int(self.get_parameter("red_min_saturation").value), + min_value=int(self.get_parameter("red_min_value").value), + morph_kernel_px=int(self.get_parameter("red_morph_kernel_px").value), + ) + + def _target_class_names(self) -> list[str]: + raw = self.get_parameter("target_class_names").value + if isinstance(raw, str): + values = raw.replace(";", ",").split(",") + else: + values = list(raw) + return [str(value).strip().lower() for value in values if str(value).strip()] + + def _marker_type(self) -> str: + return str(self.get_parameter("marker_type").value).strip().lower() + + @staticmethod + def _is_better_detection( + candidate: LidDetection2D, + best: LidDetection2D | None, + selection_policy: str, + ) -> bool: + if best is None: + return True + if selection_policy == "largest_bbox": + if candidate.area != best.area: + return candidate.area > best.area + return candidate.confidence > best.confidence + if abs(candidate.confidence - best.confidence) > 1e-9: + return candidate.confidence > best.confidence + return candidate.area > best.area + + def _depth_window_size(self) -> int: + configured = max(int(self.get_parameter("depth_window_size").value), 1) + if configured % 2 == 0: + configured += 1 + return configured + + def _source_frame(self, info: CameraInfo, msg: Image) -> str: + configured = str(self.get_parameter("source_frame").value).strip() + return configured or info.header.frame_id or msg.header.frame_id + + def _depth_scale(self) -> float | None: + mode = str(self.get_parameter("depth_scale_mode").value).strip().lower() + encoding = self._latest_depth_encoding + if mode == "manual": + depth_scale = float(self.get_parameter("depth_scale").value) + if depth_scale <= 0.0: + self.get_logger().error( + f"Rejecting lid projection: manual depth_scale must be positive, got {depth_scale}" + ) + return None + return depth_scale + if mode != "auto": + self.get_logger().error( + f"Rejecting lid projection: unsupported depth_scale_mode={mode!r}; use 'auto' or 'manual'" + ) + return None + scale = YoloTumblerDetectorNode._auto_depth_scales().get(encoding) + if scale is None: + self.get_logger().error( + f"Rejecting lid projection: unsupported depth encoding {encoding!r} in auto mode" + ) + return None + return scale + + def _log_depth_scale(self, depth_scale: float) -> None: + mode = str(self.get_parameter("depth_scale_mode").value).strip().lower() + key = f"{self._latest_depth_encoding}:{mode}:{depth_scale:.9g}" + if key == self._last_depth_scale_log: + return + self._last_depth_scale_log = key + self.get_logger().info( + "Lid depth scale selected: " + f"encoding={self._latest_depth_encoding} mode={mode} scale={depth_scale:.6g}" + ) + + 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 + output.grasp_pose = Pose() + output.cup_mouth_center = Pose() + output.confidence = 0.0 + output.status = status + output.source = str(self.get_parameter("source").value) + self._pub.publish(output) + + def _show_preview( + self, + image: np.ndarray, + status: str, + lid: LidDetection2D | None = None, + marker: ArucoMarker | RedCircle | None = None, + depth_m: float | None = None, + valid: bool = False, + ) -> None: + if not bool(self.get_parameter("show_preview").value): + return + + frame = image.copy() + color = (0, 220, 0) if valid else (0, 180, 255) + if lid is not None: + cv2.rectangle( + frame, + (lid.roi.x_min, lid.roi.y_min), + (lid.roi.x_max, lid.roi.y_max), + color, + 2, + ) + cv2.putText( + frame, + f"{lid.class_name} {lid.confidence:.2f}", + (lid.roi.x_min, max(24, lid.roi.y_min - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + color, + 2, + cv2.LINE_AA, + ) + if isinstance(marker, ArucoMarker): + corners = marker.corners.astype(np.int32).reshape(-1, 1, 2) + cv2.polylines(frame, [corners], True, (255, 0, 255), 2, cv2.LINE_AA) + cv2.circle(frame, (marker.center_u, marker.center_v), 5, (255, 0, 255), -1) + cv2.putText( + frame, + f"aruco {marker.marker_id}", + (marker.center_u + 8, max(24, marker.center_v - 8)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + (255, 0, 255), + 2, + cv2.LINE_AA, + ) + elif isinstance(marker, RedCircle): + cv2.circle(frame, (marker.center_u, marker.center_v), 5, (0, 0, 255), -1) + cv2.circle( + frame, + (marker.center_u, marker.center_v), + max(int(round(marker.radius_px)), 6), + (0, 0, 255), + 2, + ) + + short_status = status if len(status) <= 150 else status[:147] + "..." + lines = [ + "p: grip request ESC/q: quit", + f"status: {short_status}", + ] + if depth_m is not None: + lines.append(f"depth: {depth_m:.3f} m") + for index, text in enumerate(lines): + y = 26 + index * 24 + cv2.putText( + frame, + text, + (10, y), + cv2.FONT_HERSHEY_SIMPLEX, + 0.58, + (230, 230, 230), + 2, + cv2.LINE_AA, + ) + + window = str(self.get_parameter("preview_window_name").value) + if not self._preview_window_created: + cv2.namedWindow(window) + self._preview_window_created = True + 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(trigger_source="p_key") + elif key in (27, ord("q"), ord("Q")): + cv2.destroyWindow(window) + if rclpy.ok(): + rclpy.shutdown() + + 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) + stamp = self._latest_valid_stamp + payload = { + "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._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") + + def _aruco_only_after_grip_request_active(self) -> bool: + if self._marker_type() != "aruco": + return False + if not bool(self.get_parameter("allow_aruco_only_after_grip_request").value): + return False + if self._last_accepted_grip_request_time is None: + return False + window_sec = max( + float(self.get_parameter("aruco_only_after_grip_request_sec").value), + 0.0, + ) + return time.monotonic() - self._last_accepted_grip_request_time <= window_sec + + @staticmethod + def _status_is_aruco_only(status: str) -> bool: + return "class=aruco_marker" in status and "bbox=none" in status + + @staticmethod + def _pose_at( + x: float, + y: float, + z: float, + qx: float, + qy: float, + qz: float, + qw: float, + ) -> Pose: + pose = Pose() + pose.position.x = x + pose.position.y = y + pose.position.z = z + pose.orientation.x = qx + pose.orientation.y = qy + pose.orientation.z = qz + pose.orientation.w = qw + return pose + + +def main(args=None): + rclpy.init(args=args) + node = LidStickerDetectorNode() + try: + rclpy.spin(node) + finally: + if node._preview_window_created: + cv2.destroyAllWindows() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py b/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py index e49183d..86ff9a2 100644 --- a/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py +++ b/src/azas_perception/azas_perception/yolo_cup_pick_legacy_node.py @@ -167,7 +167,7 @@ def __init__(self): self.declare_parameter( "model_path", - "/home/ssu/Azas/data/yolo_runs/cup_yolov8n_ft1/weights/best.pt", + "/home/ssu/Azas/local_models/best.pt", ) self.declare_parameter("conf", 0.35) self.declare_parameter("imgsz", 640) @@ -1115,11 +1115,11 @@ def pick_and_place_side(self, base_xyz): make_pose(stage_xy[0], stage_xy[1], lift_z, side_ori), ), ( - "lower at outside side-staging pose", - make_pose(stage_xy[0], stage_xy[1], pre_z, side_ori), + "move above side pre-grasp", + make_pose(pre_xy[0], pre_xy[1], lift_z, side_ori), ), ( - "move horizontally to side pre-grasp", + "lower at side pre-grasp", make_pose(pre_xy[0], pre_xy[1], pre_z, side_ori), ), ] 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/hand_eye_calibrate_data.json b/src/azas_perception/config/hand_eye_calibrate_data.json new file mode 100644 index 0000000..3b39ed6 --- /dev/null +++ b/src/azas_perception/config/hand_eye_calibrate_data.json @@ -0,0 +1,222 @@ +{ + "poses": [ + [ + 221.05630493164062, + 6.705246925354004, + 710.15234375, + 164.198486328125, + -165.02239990234375, + 165.4091339111328 + ], + [ + 405.1767578125, + 71.18907165527344, + 594.7964477539062, + 140.5550537109375, + -166.02862548828125, + 132.96957397460938 + ], + [ + 336.2991943359375, + -16.333118438720703, + 673.9231567382812, + 173.78616333007812, + -160.34727478027344, + 177.84498596191406 + ], + [ + 417.8628845214844, + -9.401564598083496, + 615.445556640625, + 140.5615234375, + -163.58529663085938, + 144.56784057617188 + ], + [ + 268.9980163574219, + 8.997318267822266, + 717.8897094726562, + 0.08443408459424973, + 156.20603942871094, + -0.46618667244911194 + ], + [ + 498.2254333496094, + 7.614185810089111, + 498.24560546875, + 116.66268920898438, + -170.94503784179688, + 116.54146575927734 + ], + [ + 574.1867065429688, + 4.65823221206665, + 367.4131774902344, + 82.19157409667969, + -167.53955078125, + 80.15242004394531 + ], + [ + 469.4290466308594, + 210.9202880859375, + 433.7089538574219, + 102.12969970703125, + -146.82415771484375, + 81.4920883178711 + ], + [ + 429.2041015625, + 312.6830749511719, + 408.58197021484375, + 117.97481536865234, + -133.7615509033203, + 94.16188049316406 + ], + [ + 415.47125244140625, + -207.99302673339844, + 453.58544921875, + 56.25100326538086, + 159.95616149902344, + 103.75908660888672 + ], + [ + 370.5456237792969, + -354.7012634277344, + 370.65509033203125, + 61.06536865234375, + 140.49635314941406, + 111.80536651611328 + ], + [ + 389.710693359375, + -342.3162536621094, + 267.1951904296875, + 65.70675659179688, + 134.34848022460938, + 113.0377197265625 + ], + [ + 450.8240661621094, + -372.1325378417969, + 169.96530151367188, + 64.94342041015625, + 123.8027572631836, + 85.17684173583984 + ], + [ + 449.6758728027344, + -385.50775146484375, + 104.47571563720703, + 65.98688507080078, + 114.76274871826172, + 88.73485565185547 + ], + [ + 466.9466247558594, + -71.13639068603516, + 480.5066223144531, + 162.52655029296875, + -166.7830352783203, + 127.06317901611328 + ], + [ + 254.47337341308594, + -15.2769136428833, + 724.24267578125, + 172.8742218017578, + -160.2312469482422, + 133.44033813476562 + ], + [ + 227.99472045898438, + 87.75064849853516, + 450.9030456542969, + 166.56356811523438, + -149.71563720703125, + -90.7806625366211 + ], + [ + 399.2574157714844, + 375.46826171875, + 360.9875183105469, + 113.01872253417969, + -139.63441467285156, + -99.8529052734375 + ], + [ + 493.7841491699219, + 299.1468200683594, + 307.07696533203125, + 97.36504364013672, + -144.11923217773438, + -104.9944076538086 + ], + [ + 484.96319580078125, + 376.46563720703125, + 201.52584838867188, + 96.89183807373047, + -124.49008178710938, + -105.72123718261719 + ], + [ + 521.7988891601562, + 344.7679748535156, + 156.16583251953125, + 90.7044677734375, + -120.58012390136719, + -106.06925964355469 + ], + [ + 545.91845703125, + 348.6623229980469, + 118.76066589355469, + 88.63636016845703, + -118.8595199584961, + -90.31320190429688 + ], + [ + 182.53598022460938, + 82.40025329589844, + 752.2138061523438, + 8.560945510864258, + 154.18746948242188, + -151.27085876464844 + ], + [ + 199.99851989746094, + 173.36502075195312, + 219.6682891845703, + 148.2804718017578, + -133.68841552734375, + -89.5671615600586 + ] + ], + "file_name": [ + "221.05630493164062_6.705246925354004_710.15234375.jpg", + "405.1767578125_71.18907165527344_594.7964477539062.jpg", + "336.2991943359375_-16.333118438720703_673.9231567382812.jpg", + "417.8628845214844_-9.401564598083496_615.445556640625.jpg", + "268.9980163574219_8.997318267822266_717.8897094726562.jpg", + "498.2254333496094_7.614185810089111_498.24560546875.jpg", + "574.1867065429688_4.65823221206665_367.4131774902344.jpg", + "469.4290466308594_210.9202880859375_433.7089538574219.jpg", + "429.2041015625_312.6830749511719_408.58197021484375.jpg", + "415.47125244140625_-207.99302673339844_453.58544921875.jpg", + "370.5456237792969_-354.7012634277344_370.65509033203125.jpg", + "389.710693359375_-342.3162536621094_267.1951904296875.jpg", + "450.8240661621094_-372.1325378417969_169.96530151367188.jpg", + "449.6758728027344_-385.50775146484375_104.47571563720703.jpg", + "466.9466247558594_-71.13639068603516_480.5066223144531.jpg", + "254.47337341308594_-15.2769136428833_724.24267578125.jpg", + "227.99472045898438_87.75064849853516_450.9030456542969.jpg", + "399.2574157714844_375.46826171875_360.9875183105469.jpg", + "493.7841491699219_299.1468200683594_307.07696533203125.jpg", + "484.96319580078125_376.46563720703125_201.52584838867188.jpg", + "521.7988891601562_344.7679748535156_156.16583251953125.jpg", + "545.91845703125_348.6623229980469_118.76066589355469.jpg", + "182.53598022460938_82.40025329589844_752.2138061523438.jpg", + "199.99851989746094_173.36502075195312_219.6682891845703.jpg" + ] +} \ No newline at end of file diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index 6086c3b..e7470ab 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -1,128 +1,157 @@ -# 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: draft_probe_estimate_not_enabled - body_bottom_z_m: 0.000 + 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.020 - y: 0.020 - z: 0.020 - + x: 0.02 + y: 0.02 + z: 0.02 + nozzle_merge_policy: single horizontal spout cuboid uses the same X/Y footprint + dimensions and orientation as dispenser_combined_body_box front_hold_poses: - # Cup hold/front limit poses. Cup pose itself still comes from vision. dispenser_1: - position_xyz_m: [0.555, -0.100, 0.093] - quaternion_xyzw: [0.214, 0.703, 0.667, 0.122] - rpy_deg: [94.657, -6.597, 152.161] + 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.549, -0.150, 0.097] - quaternion_xyzw: [0.239, 0.695, 0.660, 0.156] - rpy_deg: [94.584, -5.669, 147.296] + 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.527, -0.204, 0.107] - quaternion_xyzw: [0.274, 0.689, 0.642, 0.194] - rpy_deg: [95.793, -4.872, 141.013] + 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.517, -0.235, 0.109] - quaternion_xyzw: [0.290, 0.682, 0.636, 0.215] - rpy_deg: [95.681, -4.333, 137.869] - + 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: - body_left_probe: - position_xyz_m: [0.533, 0.005, 0.095] - quaternion_xyzw: [0.362, 0.753, 0.526, 0.159] - rpy_deg: [113.625, -8.080, 133.982] - body_right_probe: - position_xyz_m: [0.502, -0.265, 0.095] - quaternion_xyzw: [0.403, 0.630, 0.575, 0.332] - rpy_deg: [96.862, -2.610, 117.065] - body_back_right_probe: - position_xyz_m: [0.811, -0.165, 0.120] - quaternion_xyzw: [-0.010, 0.701, 0.519, 0.489] - rpy_deg: [88.620, 44.063, 136.576] - body_top_right_probe: - position_xyz_m: [0.757, -0.184, 0.242] - quaternion_xyzw: [0.586, 0.360, 0.573, 0.446] - rpy_deg: [86.632, -20.555, 84.875] - head_top_1_probe: - position_xyz_m: [0.742, 0.066, 0.413] - quaternion_xyzw: [-0.042, 0.712, -0.047, 0.700] - rpy_deg: [-97.548, 82.646, -97.947] - head_front_probe: - position_xyz_m: [0.683, 0.029, 0.452] - quaternion_xyzw: [-0.397, 0.574, -0.374, 0.610] - rpy_deg: [-88.393, 23.817, -86.216] - + 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.2 + - 0.313 + rpy_deg: + - 137.024 + - 8.372 + - 86.35 estimated_collision_objects: - dispenser_body_box: + dispenser_combined_body_box: type: box frame_id: base_link - # Legacy broad axis-aligned estimate. Kept visible as a faint comparison - # marker only because it overlaps the taught front-hold points. - center_xyz_m: [0.6815, -0.1300, 0.1310] - size_xyz_m: [0.2990, 0.3100, 0.2620] + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 bounds_xyz_m: - min: [0.5320, -0.2850, 0.0000] - max: [0.8310, 0.0250, 0.2620] - orientation_xyzw: [0.0, 0.0, 0.0, 1.0] - publish_to_planning_scene: false - enabled_for_real_motion: false - dispenser_1_body_box_v2: - type: box - frame_id: base_link - # Local x axis follows the dispenser depth direction measured from the - # front-hold line toward the body/back probe. Local y follows the row. - center_xyz_m: [0.6946, -0.1393, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_2_body_box_v2: - type: box - frame_id: base_link - center_xyz_m: [0.6886, -0.1893, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_3_body_box_v2: - type: box - frame_id: base_link - center_xyz_m: [0.6666, -0.2433, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_4_body_box_v2: - type: box - frame_id: base_link - center_xyz_m: [0.6566, -0.2743, 0.1310] - size_xyz_m: [0.2200, 0.0650, 0.2620] - orientation_xyzw: [0.0, 0.0, -0.1368, 0.9906] + 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_head_box: + dispenser_head_nozzle_merged_horizontal_spout_box: type: box frame_id: base_link - # Head/nozzle box is separated from the bottle body so the empty space under - # the pump is not blocked by one oversized collision box. - center_xyz_m: [0.7470, -0.0995, 0.3520] - size_xyz_m: [0.1680, 0.3710, 0.2400] + center_xyz_m: + - 0.721 + - -0.0155 + - 0.432 + size_xyz_m: + - 0.044 + - 0.215 + - 0.04 bounds_xyz_m: - min: [0.6630, -0.2850, 0.2320] - max: [0.8310, 0.0860, 0.4720] - orientation_xyzw: [0.0, 0.0, 0.0, 1.0] + min: + - 0.699 + - -0.123 + - 0.412 + max: + - 0.743 + - 0.092 + - 0.452 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false + '# 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_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..ba54c70 100644 --- a/src/azas_perception/setup.py +++ b/src/azas_perception/setup.py @@ -10,7 +10,13 @@ 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") + + glob("config/*.json"), + ), ], install_requires=["setuptools"], zip_safe=True, @@ -26,6 +32,8 @@ "click_pick_legacy_node = azas_perception.click_pick_legacy_node:main", "cup_detection_pose_bridge_node = azas_perception.cup_detection_pose_bridge_node:main", "gpd_grasp_adapter_node = azas_perception.gpd_grasp_adapter_node:main", + "hand_eye_static_tf_node = azas_perception.hand_eye_static_tf_node:main", + "lid_sticker_detector_node = azas_perception.lid_sticker_detector_node:main", "realsense_data_collector_legacy_node = azas_perception.realsense_data_collector_legacy_node:main", "simulated_cup_detection_node = azas_perception.simulated_cup_detection_node:main", "yolo_cup_pick_legacy_node = azas_perception.yolo_cup_pick_legacy_node:main", 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..09c8ede 100644 --- a/src/azas_perception/test/test_depth_and_detection_logic.py +++ b/src/azas_perception/test/test_depth_and_detection_logic.py @@ -1,14 +1,47 @@ import pytest +import cv2 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.lid_marker import ( + ImageRoi, + RedCircleConfig, + detect_aruco_marker, + detect_red_circle_marker, + quaternion_from_lid_normal, +) from azas_perception.yolo_tumbler_detector_node import ( BboxHeightStats, Detection2D, YoloTumblerDetectorNode, + default_yolo_model_path, ) +def _install_aruco_generate_image_marker_compat(): + aruco = getattr(cv2, "aruco", None) + if aruco is None or hasattr(aruco, "generateImageMarker") or not hasattr(aruco, "drawMarker"): + return + + def generate_image_marker(dictionary, marker_id, side_pixels, img=None, borderBits=1): + marker = np.zeros((int(side_pixels), int(side_pixels)), dtype=np.uint8) + aruco.drawMarker(dictionary, int(marker_id), int(side_pixels), marker, int(borderBits)) + if img is not None: + img[...] = marker + return img + return marker + + aruco.generateImageMarker = generate_image_marker + + +_install_aruco_generate_image_marker_compat() + + def test_pixel_depth_to_camera_point_projects_metric_coordinates(): point = pixel_depth_to_camera_point( 330, @@ -48,6 +81,93 @@ def test_largest_bbox_policy_uses_confidence_as_tie_breaker(): ) +def test_detect_red_circle_marker_uses_lid_roi(): + image = np.zeros((100, 100, 3), dtype=np.uint8) + image[42:59, 42:59, 2] = 255 + roi = ImageRoi(20, 20, 80, 80) + + marker = detect_red_circle_marker(image, roi, RedCircleConfig(min_area_px=20.0)) + + assert marker is not None + assert marker.center_u == pytest.approx(50, abs=1) + assert marker.center_v == pytest.approx(50, abs=1) + assert marker.circularity > 0.7 + + +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) + 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) + + marker = detect_aruco_marker(image, roi, dictionary_name="DICT_4X4_50", marker_id=7) + + assert marker is not None + assert marker.marker_id == 7 + assert marker.center_u == pytest.approx(80, abs=1) + assert marker.center_v == pytest.approx(80, abs=1) + 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([ + 2.0 * (qx * qz + qy * qw), + 2.0 * (qy * qz - qx * qw), + 1.0 - 2.0 * (qx * qx + qy * qy), + ]) + + assert local_z == pytest.approx(np.array([0.0, 0.0, -1.0])) + + def test_bbox_height_stats_use_empty_table_depth_difference(): table = np.full((8, 8), 1.0, dtype=np.float32) cup = table.copy() @@ -223,3 +343,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) diff --git a/src/azas_perception/test/test_hand_eye_static_tf_node.py b/src/azas_perception/test/test_hand_eye_static_tf_node.py new file mode 100644 index 0000000..9536dc3 --- /dev/null +++ b/src/azas_perception/test/test_hand_eye_static_tf_node.py @@ -0,0 +1,43 @@ +import numpy as np +import pytest + +from azas_perception.hand_eye_static_tf_node import ( + compose_parent_to_published_child, + load_hand_eye_matrix, + matrix_from_quaternion, + quaternion_from_matrix, +) + + +def test_load_hand_eye_matrix_scales_legacy_mm_translation(tmp_path): + matrix = np.eye(4) + matrix[:3, 3] = [34.0, 57.0, 11.0] + path = tmp_path / "T_gripper2camera.npy" + np.save(path, matrix) + + loaded = load_hand_eye_matrix(path, translation_scale=0.001) + + assert loaded[:3, 3] == pytest.approx([0.034, 0.057, 0.011]) + + +def test_quaternion_matrix_roundtrip_for_180_degree_rotation(): + rotation = np.diag([-1.0, -1.0, 1.0]) + + qx, qy, qz, qw = quaternion_from_matrix(rotation) + restored = matrix_from_quaternion(qx, qy, qz, qw) + + assert restored == pytest.approx(rotation) + + +def test_compose_parent_to_published_child_preserves_matrix_child_pose(): + parent_from_optical = np.eye(4) + parent_from_optical[:3, 3] = [0.10, 0.20, 0.30] + camera_link_from_optical = np.eye(4) + camera_link_from_optical[:3, 3] = [0.01, 0.02, 0.03] + + parent_from_camera_link = compose_parent_to_published_child( + parent_from_optical, + camera_link_from_optical, + ) + + assert parent_from_camera_link @ camera_link_from_optical == pytest.approx(parent_from_optical) 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..369db19 --- /dev/null +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -0,0 +1,1352 @@ +from __future__ import annotations + +import ast +import os +import re +import shlex +import signal +import subprocess +import sys +import threading +import time +import traceback +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 + +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: + route: str + status: str + 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") + + 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", "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") + 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", 3.5) + 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", "/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 정도 어긋나는 실측 보정값 + 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) + 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( + "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:-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 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("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.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) + 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("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 " + "--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") + 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 + 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._stage_failure_reasons: dict[str, str] = {} + self._resume_store: AutoFlowResumeStore | None = None + + 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: color scan -> observe -> open -> classify -> route") + perception = None + try: + 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._run_resumable_stage("observe", lambda: self._move_observe("initial observe")): + return 1 + if not self._run_resumable_stage("open_gripper", lambda: self._open_gripper("initial gripper full-open")): + return 1 + 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_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_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 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") + 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 _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().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 + 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 + + 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(3.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" + 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) + except Exception as exc: + 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] + + @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() + cmd = self._launch_command(str(self.get_parameter("side_launch").value)) + 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", + "return_home_after_task:=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", + "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:=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", + "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", + "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)}", + 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}", + ]) + cmd.extend(self._split_extra_args(str(self.get_parameter("side_extra_args").value))) + 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})") + 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 _run_color_scan_sequence(self) -> bool: + 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 + 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") + + 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 + 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}") + 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) + 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}" + ) + 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 = 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 _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() + 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 = self._motion_service_prefix() + 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 _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 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: + 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] + + @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, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + 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() + return proc + + 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 + + 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) + 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: + 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") + + 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) + 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") + + 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 + 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 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) + # 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')}") + + 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/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..5fac3e0 --- /dev/null +++ b/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py @@ -0,0 +1,320 @@ +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", + "human_handover", +) + +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", + "human_handover": "MediaPipe palm handover", +} + + +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, + "human_handover_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/azas_task_manager/cocktail_workflow_plan.py b/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py index 7f64036..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 @@ -36,13 +36,13 @@ 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", - "accept symbolic recipe and ordered dispenser IDs", - required_inputs=("/azas/voice/recipe_decision",), - produces=("ordered_dispenser_ids",), + "accept symbolic recipe and ordered dispenser color targets", + required_inputs=("/azas/voice/confirmed_recipe_decision",), + produces=("ordered_dispenser_colors",), ), TaskStep( "VERIFY_CUP_AND_LID_DETECTION", @@ -52,13 +52,13 @@ def build_cocktail_steps(dispenser_ids: list[str]) -> list[TaskStep]: ), TaskStep( "VERIFY_CALIBRATION", - "require measured camera frame, base frame, TCP, cup offset, dispenser ID pose mapping, and safety bounds", + "require measured camera frame, base frame, TCP, cup offset, dispenser color pose mapping, and safety bounds", required_inputs=( "calibration.yaml", "safety.yaml", "base_link<-camera_frame TF", "tcp_to_cup_mouth_m", - "dispenser_outlets.1..4 outlet_pose and press_pose", + "dispenser_outlets.red|yellow|green|blue outlet_pose and press_pose", ), produces=("calibration_ready",), command="tools/checks/check_real_motion_config.sh", @@ -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/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", ], 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_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/src/azas_voice/azas_voice/command_parser.py b/src/azas_voice/azas_voice/command_parser.py index ccedc77..8755bb3 100644 --- a/src/azas_voice/azas_voice/command_parser.py +++ b/src/azas_voice/azas_voice/command_parser.py @@ -4,14 +4,21 @@ 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, + recipe_amounts, ) @@ -25,9 +32,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 +46,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: @@ -47,13 +61,100 @@ 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 + 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, ...]: @@ -70,18 +171,132 @@ 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) +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] + amounts = recipe_amounts(recipe_id) + description = _recipe_description(recipe_id) confirmation = ( - f"오늘 기분에는 {_recipe_name(recipe_id)}를 추천합니다. " - f"사용 디스펜서: {', '.join(dispenser_ids)}. 진행할까요?" + f"{_recipe_name(recipe_id)}를 추천드릴게요. " + f"{description} 진행할까요?" + ) + 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: + 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, ) - return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) def parse_recipe_command(text: str) -> RecipeDecision: @@ -91,18 +306,48 @@ 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) + 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 _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 _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 _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): + return RecipeDecision(True, utterance, normalized, "confirm", None, (), "선택한 칵테일 제조를 확인했습니다.") + if recipe_id is None and not dispenser_ids: return RecipeDecision( False, @@ -115,11 +360,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)} 요청을 인식했습니다. 사용 디스펜서: {dispenser_text}. 진행할까요?" - return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) + confirmation = f"{_recipe_name(recipe_id)} 요청을 인식했습니다. 진행할까요?" + return RecipeDecision( + True, + utterance, + normalized, + "make_cocktail", + recipe_id, + dispenser_ids, + confirmation, + dispenser_amounts=amounts, + ) diff --git a/src/azas_voice/azas_voice/conversation_manager_node.py b/src/azas_voice/azas_voice/conversation_manager_node.py new file mode 100644 index 0000000..1cb4be3 --- /dev/null +++ b/src/azas_voice/azas_voice/conversation_manager_node.py @@ -0,0 +1,140 @@ +from __future__ import annotations + +import copy +import json +import time + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + + +class ConversationManagerNode(Node): + """Keep pending recipe state and emit confirmed execution requests.""" + + def __init__(self): + super().__init__("conversation_manager_node") + + 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 + self._pending_at = 0.0 + self._timeout_s = float(self.get_parameter("pending_timeout_s").value) + + self._confirmation_pub = self.create_publisher( + String, + str(self.get_parameter("confirmation_topic").value), + 10, + ) + self._confirmed_pub = self.create_publisher( + String, + 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), + self._on_decision, + 10, + ) + + self.get_logger().info( + "Conversation manager ready: " + f"{self.get_parameter('decision_topic').value} -> " + f"{self.get_parameter('confirmed_decision_topic').value}" + ) + + def _on_decision(self, msg: String) -> None: + try: + decision = json.loads(msg.data) + except json.JSONDecodeError as exc: + self._publish_confirmation(f"음성 인식 결과를 처리하지 못했습니다: {exc}") + return + + intent = str(decision.get("intent", "unknown")) + if intent == "make_cocktail": + self._handle_make_cocktail(decision) + elif intent == "confirm": + self._handle_confirm(decision) + 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: + self._publish_confirmation("메뉴를 다시 말씀해주세요.") + + def _handle_make_cocktail(self, decision: dict[str, object]) -> None: + if not decision.get("valid"): + self._pending = None + self._publish_confirmation("메뉴를 다시 말씀해주세요.") + return + + if not decision.get("recipe_id") and not decision.get("dispenser_ids"): + self._pending = None + self._publish_confirmation("선택할 메뉴를 찾지 못했습니다.") + return + + self._pending = copy.deepcopy(decision) + self._pending_at = time.monotonic() + self._publish_confirmation(str(decision.get("confirmation") or "진행할까요?")) + + def _handle_confirm(self, decision: dict[str, object]) -> None: + if self._pending is None: + self._publish_confirmation("먼저 메뉴를 선택해주세요.") + return + + if time.monotonic() - self._pending_at > self._timeout_s: + self._pending = None + self._publish_confirmation("이전 주문이 만료되었습니다. 메뉴를 다시 말씀해주세요.") + return + + confirmed = copy.deepcopy(self._pending) + confirmed["confirmed"] = True + confirmed["confirmed_by"] = "voice" + confirmed["confirm_utterance"] = decision.get("utterance", "") + confirmed["confirmation"] = "제조를 시작합니다." + self._pending = None + + msg = String() + msg.data = json.dumps(confirmed, ensure_ascii=False) + self._confirmed_pub.publish(msg) + 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 + self._confirmation_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = ConversationManagerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() 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 c7e1fe3..bfc5edf 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -11,25 +11,61 @@ 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, + recipe_amounts, +) -ALLOWED_INTENTS = {"make_cocktail", "confirm", "cancel", "unknown"} -ALLOWED_DISPENSERS = {"1", "2", "3", "4"} +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", + "2": "yellow", + "3": "green", + "4": "blue", +} +ALLOWED_TRAITS = set().union(*DISPENSER_TRAITS.values()) def _normalize_dispenser_id(value: object) -> str: raw = str(value).strip() - if raw in ALLOWED_DISPENSERS: - return raw + if raw in DISPENSER_NUMBER_TO_COLOR: + return DISPENSER_NUMBER_TO_COLOR[raw] normalized = "".join(raw.lower().split()) for dispenser_id, aliases in COLOR_ALIASES.items(): + if dispenser_id == normalized: + return dispenser_id if any("".join(alias.lower().split()) == normalized for alias in aliases): return dispenser_id 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: @@ -46,42 +82,135 @@ 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: return _fallback_decision(text, f"invalid_intent:{intent}") + fallback = parse_recipe_command(text) + 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 + 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 recipe_id not in RECIPE_DISPENSERS 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 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") - 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_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 ( + 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( + 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 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: + 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: - color_text = ", ".join(dispenser_ids) if dispenser_ids else "configured recipe dispensers" recipe_name = RECIPE_DISPLAY_NAMES.get(str(recipe_id), str(recipe_id)) - confirmation = f"{recipe_name} 요청을 인식했습니다. 사용 디스펜서: {color_text}. 진행할까요?" + 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(), @@ -91,6 +220,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, ) @@ -112,6 +243,7 @@ def __init__(self): self.declare_parameter("base_url", "https://api.openai.com/v1") self.declare_parameter("model", "gpt-4o-mini") self.declare_parameter("request_timeout_sec", 8.0) + self.declare_parameter("publish_confirmation", True) self._decision_pub = self.create_publisher( String, @@ -129,6 +261,7 @@ def __init__(self): self._on_stt, 10, ) + self._publish_confirmation = bool(self.get_parameter("publish_confirmation").value) self.get_logger().info( "LLM recipe mapper ready: " f"enable_llm={bool(self.get_parameter('enable_llm').value)} " @@ -141,7 +274,7 @@ def _on_stt(self, msg: String) -> None: payload.data = json.dumps(decision.to_dict(), ensure_ascii=False) self._decision_pub.publish(payload) - if decision.confirmation: + if self._publish_confirmation and decision.confirmation: confirmation = String() confirmation.data = decision.confirmation self._confirmation_pub.publish(confirmation) @@ -176,17 +309,25 @@ 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 numbers; infer them internally. " - "If the user describes mood or asks for a recommendation, choose one recipe_01..recipe_16. " - "Allowed dispenser_ids: 1, 2, 3, 4 only. " + "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 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. " + "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." ), }, {"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 750c3aa..f90b7df 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -1,72 +1,188 @@ +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 = { - "1": ("1번", "일번", "디스펜서1", "디스펜서일", "노랑", "노란색", "옐로우", "yellow", "노란"), - "2": ("2번", "이번", "디스펜서2", "디스펜서이", "빨강", "빨간색", "레드", "red", "빨간"), - "3": ("3번", "삼번", "디스펜서3", "디스펜서삼", "파랑", "파란색", "블루", "blue", "파란"), - "4": ("4번", "사번", "디스펜서4", "디스펜서사", "초록", "초록색", "그린", "green", "녹색"), + "red": ("1번", "일번", "디스펜서1", "디스펜서일", "빨강", "빨간색", "레드", "red", "빨간"), + "yellow": ("2번", "이번", "디스펜서2", "디스펜서이", "노랑", "노란색", "옐로우", "yellow", "노란"), + "green": ("3번", "삼번", "디스펜서3", "디스펜서삼", "초록", "초록색", "그린", "green", "녹색"), + "blue": ("4번", "사번", "디스펜서4", "디스펜서사", "파랑", "파란색", "블루", "blue", "파란"), } -# Backward-compatible import name. Parser output still uses fixed dispenser -# numbers only; color words are accepted only as aliases for those numbers. +# Backward-compatible import name. Parser output uses color strings because +# 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"), - "recipe_03": ("3번", "삼번", "레시피3", "recipe3", "recipe_03"), - "recipe_04": ("4번", "사번", "레시피4", "recipe4", "recipe_04"), - "recipe_05": ("5번", "오번", "레시피5", "recipe5", "recipe_05"), - "recipe_06": ("6번", "육번", "레시피6", "recipe6", "recipe_06"), - "recipe_07": ("7번", "칠번", "레시피7", "recipe7", "recipe_07"), - "recipe_08": ("8번", "팔번", "레시피8", "recipe8", "recipe_08"), - "recipe_09": ("9번", "구번", "레시피9", "recipe9", "recipe_09"), - "recipe_10": ("10번", "십번", "레시피10", "recipe10", "recipe_10"), - "recipe_11": ("11번", "십일번", "레시피11", "recipe11", "recipe_11"), - "recipe_12": ("12번", "십이번", "레시피12", "recipe12", "recipe_12"), - "recipe_13": ("13번", "십삼번", "레시피13", "recipe13", "recipe_13"), - "recipe_14": ("14번", "십사번", "레시피14", "recipe14", "recipe_14"), - "recipe_15": ("15번", "십오번", "레시피15", "recipe15", "recipe_15"), - "recipe_16": ("16번", "십육번", "레시피16", "recipe16", "recipe_16"), + "recipe_01": ("1번", "일번", "레시피1", "recipe1", "recipe_01", "레드메뉴", "빨강메뉴", "빨간색메뉴"), + "recipe_02": ("2번", "이번", "레시피2", "recipe2", "recipe_02", "옐로우메뉴", "노랑메뉴", "노란색메뉴"), + "recipe_03": ("3번", "삼번", "레시피3", "recipe3", "recipe_03", "그린메뉴", "초록메뉴", "초록색메뉴"), + "recipe_04": ("4번", "사번", "레시피4", "recipe4", "recipe_04", "블루메뉴", "파랑메뉴", "파란색메뉴"), } RECIPE_DISPLAY_NAMES = { - "recipe_01": "선셋 믹스", - "recipe_02": "블루 라군", - "recipe_03": "그린 스파클", - "recipe_04": "레드 펀치", - "recipe_05": "옐로우 브리즈", - "recipe_06": "시트러스 쿨러", - "recipe_07": "베리 블루", - "recipe_08": "민트 선라이즈", - "recipe_09": "트로피컬 무드", - "recipe_10": "라임 레드", - "recipe_11": "오션 옐로우", - "recipe_12": "포레스트 펀치", - "recipe_13": "스윗 밸런스", - "recipe_14": "프레시 믹스", - "recipe_15": "파티 컬러", - "recipe_16": "랜덤 시그니처", + "recipe_01": "레드 메뉴", + "recipe_02": "옐로우 메뉴", + "recipe_03": "그린 메뉴", + "recipe_04": "블루 메뉴", +} + +RECIPE_DESCRIPTIONS = { + "recipe_01": "주스 중심이라 과일감이 선명하고 가볍게 마시기 좋습니다.", + "recipe_02": "시럽 중심이라 달콤하고 부드러운 느낌이 강합니다.", + "recipe_03": "리큐르 중심이라 향이 선명하고 깔끔한 여운이 있습니다.", + "recipe_04": "럼 중심이라 칵테일다운 존재감과 깊이가 있습니다.", } RECIPE_DISPENSERS = { - "recipe_01": ("1", "2"), - "recipe_02": ("3", "4"), - "recipe_03": ("4", "1"), - "recipe_04": ("2", "3"), - "recipe_05": ("1", "4"), - "recipe_06": ("1", "3"), - "recipe_07": ("2", "3", "4"), - "recipe_08": ("4", "1", "2"), - "recipe_09": ("1", "2", "3"), - "recipe_10": ("2", "4"), - "recipe_11": ("3", "1", "4"), - "recipe_12": ("4", "2"), - "recipe_13": ("1", "3", "2"), - "recipe_14": ("4", "3", "1"), - "recipe_15": ("2", "1", "4", "3"), - "recipe_16": ("3", "2", "1"), + "recipe_01": ("red",), + "recipe_02": ("yellow",), + "recipe_03": ("green",), + "recipe_04": ("blue",), } +RECIPE_AMOUNTS: dict[str, dict[str, int]] = {} +RECIPE_METADATA: dict[str, dict[str, object]] = {} MOOD_WORDS = ( "기분", @@ -95,5 +211,346 @@ "알려줘", ) -CONFIRM_WORDS = ("확인", "맞아", "맞습니다", "응", "네", "예", "좋아", "시작") +REROLL_RECOMMENDATION_WORDS = ( + "다른거", + "다른것", + "다른메뉴", + "다른걸", + "다른걸로", + "다른거로", + "말고다른", + "말고다른거", + "말고다른메뉴", + "새로추천", + "다시추천", +) + +PREFERENCE_WORDS = ( + "덜달", + "안달", + "달달", + "달게", + "달콤", + "시럽", + "상큼", + "새콤", + "신맛", + "산미", + "주스", + "세지", + "세지않", + "안세", + "부담", + "가볍", + "편한", + "기분안좋", + "안좋", + "기분좋", + "행복", + "신나", + "우울", + "힘들", + "피곤", + "술약", + "약하게", + "술강", + "강하게", + "도수", + "도수쎈", + "도수센", + "쎈거", + "센거", + "쌘거", + "더쎈", + "더센", + "더쌘", + "강한거", + "럼", + "무알콜", + "논알콜", + "알코올없이", + "술없이", + "리큐르", + "쓴맛", + "쓴술", + "술은싫", + "술싫", + "독한술", + "독한건싫", + "과일", + "과일맛", + "향", + "풍부", + "진하게", + "깔끔", +) + +CONFIRM_WORDS = ( + "확인", + "확인해", + "확인해줘", + "확정", + "확정해", + "확정해줘", + "맞아", + "맞아요", + "맞습니다", + "응", + "응응", + "네", + "넵", + "넹", + "예", + "예스", + "yes", + "ok", + "okay", + "오케이", + "오키", + "그래", + "그렇게", + "그렇게해", + "그렇게해줘", + "좋아", + "좋아요", + "좋습니다", + "좋지", + "괜찮아", + "괜찮아요", + "괜찮습니다", + "알겠어", + "알겠어요", + "알겠습니다", + "알았어", + "알았어요", + "알았습니다", + "알겠", + "알았", + "오케", + "콜", + "가자", + "가보자", + "시작", + "시작해", + "시작해줘", + "진행", + "진행해", + "진행해줘", + "만들어", + "만들어줘", + "만들어주세요", + "제조", + "제조해", + "제조해줘", + "제조해주세요", + "계속해", + "계속해줘", + "계속", +) 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/recipe_mapper_node.py b/src/azas_voice/azas_voice/recipe_mapper_node.py index 27eb5d3..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 @@ -19,10 +20,20 @@ def __init__(self): 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("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) @@ -34,11 +45,28 @@ 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) - if decision.confirmation: + if self._publish_confirmation and decision.confirmation: confirmation = String() confirmation.data = decision.confirmation self._confirmation_pub.publish(confirmation) diff --git a/src/azas_voice/azas_voice/tts_node.py b/src/azas_voice/azas_voice/tts_node.py new file mode 100644 index 0000000..e001dbc --- /dev/null +++ b/src/azas_voice/azas_voice/tts_node.py @@ -0,0 +1,201 @@ +from __future__ import annotations + +import json +import os +import queue +import shutil +import subprocess +import tempfile +import threading +import time + +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 + +try: + from gtts import gTTS + import pygame + + _GTTS_PYGAME_AVAILABLE = True +except ImportError: # pragma: no cover - depends on optional host packages + _GTTS_PYGAME_AVAILABLE = False + + +def build_ui_state(state: str, text: str = "", emotion: str = "neutral") -> str: + return json.dumps( + { + "state": state, + "emotion": emotion, + "text": text, + }, + ensure_ascii=False, + ) + + +class _SpeechEngine: + def speak(self, text: str) -> bool: + raise NotImplementedError + + def shutdown(self) -> None: + pass + + +class _SilentSpeechEngine(_SpeechEngine): + def speak(self, text: str) -> bool: + return False + + +class _GttsPygameSpeechEngine(_SpeechEngine): + def __init__(self, language: str, speech_rate: float): + if not _GTTS_PYGAME_AVAILABLE: + raise RuntimeError("gTTS/pygame are not available") + self._language = language + self._speech_rate = max(0.5, min(speech_rate, 2.0)) + self._cache: dict[tuple[str, float], str] = {} + self._ffmpeg = shutil.which("ffmpeg") + pygame.mixer.init() + + def speak(self, text: str) -> bool: + cache_key = (text, self._speech_rate) + path = self._cache.get(cache_key) + if not path or not os.path.exists(path): + fd, raw_path = tempfile.mkstemp(suffix=".mp3", prefix="azas_tts_raw_") + os.close(fd) + gTTS(text=text, lang=self._language).save(raw_path) + path = self._speed_adjusted_path(raw_path) + self._cache[cache_key] = path + + pygame.mixer.music.load(path) + pygame.mixer.music.play() + while pygame.mixer.music.get_busy(): + time.sleep(0.05) + return True + + def _speed_adjusted_path(self, raw_path: str) -> str: + if self._speech_rate == 1.0 or self._ffmpeg is None: + return raw_path + + fd, adjusted_path = tempfile.mkstemp(suffix=".mp3", prefix="azas_tts_fast_") + os.close(fd) + subprocess.run( + [ + self._ffmpeg, + "-y", + "-loglevel", + "error", + "-i", + raw_path, + "-filter:a", + f"atempo={self._speech_rate:.2f}", + adjusted_path, + ], + check=True, + ) + try: + os.unlink(raw_path) + except OSError: + pass + return adjusted_path + + def shutdown(self) -> None: + if _GTTS_PYGAME_AVAILABLE: + pygame.mixer.music.stop() + + +class TtsNode(Node): + """Speak confirmation text and publish avatar/UI conversation state.""" + + def __init__(self): + if rclpy is None or String is None: + raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") + super().__init__("tts_node") + + self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") + self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") + self.declare_parameter("language", "ko") + self.declare_parameter("enable_audio", True) + self.declare_parameter("speech_rate", 1.25) + 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) + language = str(self.get_parameter("language").value) + enable_audio = bool(self.get_parameter("enable_audio").value) + speech_rate = float(self.get_parameter("speech_rate").value) + startup_prompt = str(self.get_parameter("startup_prompt").value).strip() + + self._ui_state_pub = self.create_publisher(String, ui_state_topic, 10) + self._queue: queue.Queue[str | None] = queue.Queue() + self._engine = self._build_engine(language, enable_audio, speech_rate) + self._worker = threading.Thread(target=self._run_worker, daemon=True) + self._worker.start() + + self.create_subscription(String, confirmation_topic, self._on_confirmation, 10) + self._publish_state("idle") + if startup_prompt: + self._queue.put(startup_prompt) + self.get_logger().info( + "TTS ready: " + f"confirmation_topic={confirmation_topic} ui_state_topic={ui_state_topic} " + f"audio={'on' if not isinstance(self._engine, _SilentSpeechEngine) else 'off'} " + f"speech_rate={speech_rate:.2f}" + ) + + def _build_engine(self, language: str, enable_audio: bool, speech_rate: float) -> _SpeechEngine: + if not enable_audio: + return _SilentSpeechEngine() + try: + return _GttsPygameSpeechEngine(language, speech_rate) + except Exception as exc: + self.get_logger().warn(f"TTS audio disabled: {exc}") + return _SilentSpeechEngine() + + def _on_confirmation(self, msg: String) -> None: + text = msg.data.strip() + if not text: + return + self._queue.put(text) + + def _run_worker(self) -> None: + while rclpy is not None and rclpy.ok(): + text = self._queue.get() + if text is None: + break + + self._publish_state("speaking", text=text, emotion="friendly") + try: + spoke = self._engine.speak(text) + if not spoke: + self.get_logger().info(f"[TTS muted] {text}") + except Exception as exc: + self.get_logger().error(f"TTS playback failed: {exc}") + self._publish_state("error", text=text, emotion="concerned") + else: + self._publish_state("idle") + + def _publish_state(self, state: str, text: str = "", emotion: str = "neutral") -> None: + msg = String() + msg.data = build_ui_state(state, text, emotion) + self._ui_state_pub.publish(msg) + + def destroy_node(self): + self._queue.put(None) + self._worker.join(timeout=1.0) + self._engine.shutdown() + super().destroy_node() + + +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 = TtsNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_voice/azas_voice/voice_dispenser_executor_node.py b/src/azas_voice/azas_voice/voice_dispenser_executor_node.py new file mode 100644 index 0000000..226b62f --- /dev/null +++ b/src/azas_voice/azas_voice/voice_dispenser_executor_node.py @@ -0,0 +1,289 @@ +from __future__ import annotations + +import json +import shutil +import subprocess +import threading +from collections import deque +from dataclasses import dataclass + +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") + + +@dataclass(frozen=True) +class DispenserPressRequest: + target_dispenser: str + repeat_index: int + repeat_total: int + recipe_id: str | None + + +def _clamp_amount(value: object, default: int = 1, max_repeats: int = 3) -> int: + try: + amount = int(value) + except (TypeError, ValueError): + amount = default + return max(0, min(amount, max_repeats)) + + +def requests_from_decision( + decision: dict[str, object], + *, + max_repeats_per_dispenser: int = 3, + default_amount: int = 1, +) -> list[DispenserPressRequest]: + if decision.get("intent") != "make_cocktail": + return [] + + recipe_id = decision.get("recipe_id") + recipe_id = str(recipe_id) if recipe_id is not None else None + + raw_ids = decision.get("dispenser_ids", []) + if not isinstance(raw_ids, list): + return [] + + amounts = decision.get("dispenser_amounts", {}) + if not isinstance(amounts, dict): + amounts = {} + + requests: list[DispenserPressRequest] = [] + for raw_id in raw_ids: + dispenser_id = str(raw_id).strip() + if dispenser_id not in ALLOWED_DISPENSERS: + continue + + amount = _clamp_amount( + amounts.get(dispenser_id, default_amount), + default=default_amount, + max_repeats=max_repeats_per_dispenser, + ) + for repeat_index in range(1, amount + 1): + requests.append( + DispenserPressRequest( + target_dispenser=dispenser_id, + repeat_index=repeat_index, + repeat_total=amount, + recipe_id=recipe_id, + ) + ) + return requests + + +def build_dispenser_launch_command( + request: DispenserPressRequest, + *, + launch_file: str, + service_prefix: str, + tcp_name: str, + restore_tcp_after_run: bool, + require_tcp_for_taught_posx: bool, + allow_tcp_set_failure: bool, + joint_velocity: float, + joint_acceleration: float, + line_velocity: float, + line_acceleration: float, +) -> list[str]: + command = [ + "ros2", + "launch", + "azas_dispenser", + launch_file, + f"target_dispenser:={request.target_dispenser}", + f"service_prefix:={service_prefix}", + ] + # ros2 launch rejects an empty value ("tcp_name:="). When no TCP is named, + # omit the argument so the launch file keeps its own default. + if tcp_name: + command.append(f"tcp_name:={tcp_name}") + command += [ + f"restore_tcp_after_run:={str(restore_tcp_after_run).lower()}", + f"require_tcp_for_taught_posx:={str(require_tcp_for_taught_posx).lower()}", + f"allow_tcp_set_failure:={str(allow_tcp_set_failure).lower()}", + f"joint_velocity:={joint_velocity}", + f"joint_acceleration:={joint_acceleration}", + f"line_velocity:={line_velocity}", + f"line_acceleration:={line_acceleration}", + ] + return command + + +class VoiceDispenserExecutorNode(Node): + """Convert confirmed voice recipe JSON into azas_dispenser launch commands.""" + + def __init__(self): + super().__init__("voice_dispenser_executor_node") + self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + self.declare_parameter("status_topic", "/azas/voice/dispenser_execution_status") + self.declare_parameter("enable_hardware_execution", False) + self.declare_parameter("require_confirmed", True) + self.declare_parameter("dispenser_launch_file", "dispenser_press.launch.py") + self.declare_parameter("service_prefix", "/") + self.declare_parameter("tcp_name", "") + self.declare_parameter("restore_tcp_after_run", True) + self.declare_parameter("require_tcp_for_taught_posx", True) + self.declare_parameter("allow_tcp_set_failure", False) + self.declare_parameter("max_repeats_per_dispenser", 3) + self.declare_parameter("default_amount", 1) + self.declare_parameter("joint_velocity", 10.0) + self.declare_parameter("joint_acceleration", 10.0) + self.declare_parameter("line_velocity", 15.0) + self.declare_parameter("line_acceleration", 25.0) + + 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._queue: deque[tuple[dict[str, object], DispenserPressRequest]] = deque() + self._condition = threading.Condition() + self._worker = threading.Thread(target=self._worker_loop, daemon=True) + self._worker.start() + + self.get_logger().info( + "Voice dispenser 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 + + requests = requests_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 requests: + self._publish_status("blocked", reason="no_executable_dispenser_requests", decision=decision) + return + + with self._condition: + for request in requests: + self._queue.append((decision, request)) + self._condition.notify() + + self._publish_status( + "queued", + recipe_id=decision.get("recipe_id"), + command_count=len(requests), + targets=[request.target_dispenser for request in requests], + ) + + def _worker_loop(self) -> None: + while True: + with self._condition: + while not self._queue: + self._condition.wait() + decision, request = self._queue.popleft() + self._execute_request(decision, request) + + def _execute_request(self, decision: dict[str, object], request: DispenserPressRequest) -> None: + command = build_dispenser_launch_command( + request, + launch_file=str(self.get_parameter("dispenser_launch_file").value), + service_prefix=str(self.get_parameter("service_prefix").value), + tcp_name=str(self.get_parameter("tcp_name").value), + restore_tcp_after_run=bool(self.get_parameter("restore_tcp_after_run").value), + require_tcp_for_taught_posx=bool(self.get_parameter("require_tcp_for_taught_posx").value), + allow_tcp_set_failure=bool(self.get_parameter("allow_tcp_set_failure").value), + joint_velocity=float(self.get_parameter("joint_velocity").value), + joint_acceleration=float(self.get_parameter("joint_acceleration").value), + line_velocity=float(self.get_parameter("line_velocity").value), + line_acceleration=float(self.get_parameter("line_acceleration").value), + ) + self._publish_status( + "starting", + recipe_id=decision.get("recipe_id"), + target_dispenser=request.target_dispenser, + repeat_index=request.repeat_index, + repeat_total=request.repeat_total, + 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", + target_dispenser=request.target_dispenser, + repeat_index=request.repeat_index, + repeat_total=request.repeat_total, + command=command, + ) + return + + if shutil.which("ros2") is None: + self._publish_status("failed", target_dispenser=request.target_dispenser, reason="ros2_not_found") + self._abort_pending_requests("ros2_not_found") + return + + try: + completed = subprocess.run(command, check=False) + except OSError as exc: + self._publish_status( + "failed", + target_dispenser=request.target_dispenser, + reason="launch_failed_to_start", + error=str(exc), + ) + self._abort_pending_requests("launch_failed_to_start") + return + + if completed.returncode == 0: + self._publish_status("completed", target_dispenser=request.target_dispenser) + else: + self._publish_status( + "failed", + target_dispenser=request.target_dispenser, + returncode=completed.returncode, + ) + self._abort_pending_requests("dispenser_launch_failed") + + 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"}: + self.get_logger().warn(msg.data) + else: + self.get_logger().info(msg.data) + + def _abort_pending_requests(self, reason: str) -> None: + with self._condition: + dropped = len(self._queue) + self._queue.clear() + if dropped: + self._publish_status("aborted", reason=reason, dropped_requests=dropped) + + +def main(args=None): + if rclpy is None: + raise RuntimeError("rclpy is required to run voice_dispenser_executor_node") + rclpy.init(args=args) + node = VoiceDispenserExecutorNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() 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..6c92f24 --- /dev/null +++ b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py @@ -0,0 +1,410 @@ +from __future__ import annotations + +import json +import os +from pathlib import Path +import signal +import subprocess +import threading +from collections import deque + +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") +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의 로그 문구가 바뀌면 여기도 같이 갱신할 것) +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", "완료"), +) + + +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 + + +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). + + 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("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) + 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.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, + 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.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 + 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 + + 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( + "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), + 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, + 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, + 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, resume_mode), + daemon=True, + ).start() + + 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 + 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, + resume_mode=resume_mode, + output_tail=list(output_tail), + ) + + 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 new file mode 100644 index 0000000..3328800 --- /dev/null +++ b/src/azas_voice/azas_voice/voice_screen_node.py @@ -0,0 +1,678 @@ +from __future__ import annotations + +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 +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 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(), + "catalog": build_public_catalog(), + "last_stt": "", + "last_confirmation": "", + "ui_state": {"state": "idle", "emotion": "neutral", "text": ""}, + "decision": {}, + "confirmed_decision": {}, + "pipeline_status": {}, + "camera_status": {}, + "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.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, + 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.create_subscription( + String, + str(self.get_parameter("pipeline_status_topic").value), + 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) + 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) + 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: + 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 _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 _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( + { + "speaker": speaker, + "text": text, + "at": time.time(), + } + ) + + def _build_handler(self): + node = self + + class VoiceScreenRequestHandler(BaseHTTPRequestHandler): + def do_GET(self) -> None: + route = urlparse(self.path).path + if route in {"/", "/voice.html"}: + self._send_file(node._web_root / "voice.html") + return + if route == "/voice.css": + self._send_file(node._web_root / "voice.css") + return + if route == "/voice.js": + self._send_file(node._web_root / "voice.js") + return + 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 route != "/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_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) + 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 _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.") + rclpy.init(args=args) + node = VoiceScreenNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_voice/config/recipes.yaml b/src/azas_voice/config/recipes.yaml index 4fe5bf4..56f4d78 100644 --- a/src/azas_voice/config/recipes.yaml +++ b/src/azas_voice/config/recipes.yaml @@ -1,30 +1,235 @@ # Azas voice recipe catalog. -# Color sticker IDs are source-backed by user input: yellow, red, blue, green. +# Color sticker IDs are source-backed by user input: red, yellow, green, blue. # Ingredient assignments are still symbolic; robot/dispenser coordinates are not # stored here and must come from calibration/runtime gates. colors: - yellow: - aliases: [노랑, 노란색, 옐로우, yellow] red: + role: juice + ingredient_role: 주스 aliases: [빨강, 빨간색, 레드, red] - blue: - aliases: [파랑, 파란색, 블루, blue] + 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: 선셋 믹스, aliases: [1번, 일번, 레시피1], dispenser_ids: [yellow, red]} - recipe_02: {name: 블루 라군, aliases: [2번, 이번, 레시피2], dispenser_ids: [blue, green]} - recipe_03: {name: 그린 스파클, aliases: [3번, 삼번, 레시피3], dispenser_ids: [green, yellow]} - recipe_04: {name: 레드 펀치, aliases: [4번, 사번, 레시피4], dispenser_ids: [red, blue]} - recipe_05: {name: 옐로우 브리즈, aliases: [5번, 오번, 레시피5], dispenser_ids: [yellow, green]} - recipe_06: {name: 시트러스 쿨러, aliases: [6번, 육번, 레시피6], dispenser_ids: [yellow, blue]} - recipe_07: {name: 베리 블루, aliases: [7번, 칠번, 레시피7], dispenser_ids: [red, blue, green]} - recipe_08: {name: 민트 선라이즈, aliases: [8번, 팔번, 레시피8], dispenser_ids: [green, yellow, red]} - recipe_09: {name: 트로피컬 무드, aliases: [9번, 구번, 레시피9], dispenser_ids: [yellow, red, blue]} - recipe_10: {name: 라임 레드, aliases: [10번, 십번, 레시피10], dispenser_ids: [red, green]} - recipe_11: {name: 오션 옐로우, aliases: [11번, 십일번, 레시피11], dispenser_ids: [blue, yellow, green]} - recipe_12: {name: 포레스트 펀치, aliases: [12번, 십이번, 레시피12], dispenser_ids: [green, red]} - recipe_13: {name: 스윗 밸런스, aliases: [13번, 십삼번, 레시피13], dispenser_ids: [yellow, blue, red]} - recipe_14: {name: 프레시 믹스, aliases: [14번, 십사번, 레시피14], dispenser_ids: [green, blue, yellow]} - recipe_15: {name: 파티 컬러, aliases: [15번, 십오번, 레시피15], dispenser_ids: [red, yellow, green, blue]} - recipe_16: {name: 랜덤 시그니처, aliases: [16번, 십육번, 레시피16], dispenser_ids: [blue, red, yellow]} + recipe_01: + name: 레드 메뉴 + aliases: [1번, 일번, 레시피1, 레드메뉴, 빨강메뉴, 빨간색메뉴] + dispenser_ids: [red] + tags: [sweet, fruity, red] + mood_tags: [cheerful, energetic] + sweetness: 4 + acidity: 2 + strength: 0 + color: red + description: 달콤하고 선명한 레드 계열 메뉴 + + recipe_02: + name: 옐로우 메뉴 + aliases: [2번, 이번, 레시피2, 옐로우메뉴, 노랑메뉴, 노란색메뉴] + dispenser_ids: [yellow] + tags: [fresh, citrus, yellow] + mood_tags: [refreshing, bright] + sweetness: 3 + acidity: 4 + strength: 0 + color: yellow + description: 산뜻하고 가벼운 옐로우 계열 메뉴 + + recipe_03: + name: 그린 메뉴 + aliases: [3번, 삼번, 레시피3, 그린메뉴, 초록메뉴, 초록색메뉴] + dispenser_ids: [green] + tags: [herbal, fresh, green] + mood_tags: [calm, clean] + sweetness: 2 + acidity: 3 + strength: 0 + color: green + description: 깔끔하고 허브 느낌의 그린 계열 메뉴 + + recipe_04: + name: 블루 메뉴 + aliases: [4번, 사번, 레시피4, 블루메뉴, 파랑메뉴, 파란색메뉴] + dispenser_ids: [blue] + tags: [cool, sweet, blue] + mood_tags: [relaxed, playful] + sweetness: 4 + acidity: 1 + 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 2278b49..9eba07d 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -9,23 +9,77 @@ 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_dispenser_executor = LaunchConfiguration("use_dispenser_executor") + use_tts = LaunchConfiguration("use_tts") + enable_tts_audio = LaunchConfiguration("enable_tts_audio") + 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( [ DeclareLaunchArgument("use_live_stt", default_value="false"), DeclareLaunchArgument("use_llm", default_value="false"), + 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( + "dispenser_require_tcp_for_taught_posx", default_value="true" + ), + DeclareLaunchArgument("dispenser_joint_velocity", default_value="10.0"), + DeclareLaunchArgument("dispenser_joint_acceleration", default_value="10.0"), + DeclareLaunchArgument("dispenser_line_velocity", default_value="15.0"), + DeclareLaunchArgument("dispenser_line_acceleration", default_value="25.0"), + 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"), + 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"), + 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", name="recipe_mapper_node", output="screen", - parameters=[{"stt_topic": stt_topic}], + parameters=[ + { + "stt_topic": stt_topic, + "publish_confirmation": False, + } + ], condition=UnlessCondition(use_llm), ), Node( @@ -40,17 +94,121 @@ 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, } ], condition=IfCondition(use_llm), ), + Node( + package="azas_voice", + executable="conversation_manager_node", + name="conversation_manager_node", + output="screen", + condition=IfCondition(use_conversation_manager), + ), + Node( + package="azas_voice", + executable="voice_dispenser_executor_node", + name="voice_dispenser_executor_node", + output="screen", + parameters=[ + { + "enable_hardware_execution": ParameterValue( + LaunchConfiguration("enable_dispenser_hardware_execution"), + value_type=bool, + ), + "service_prefix": LaunchConfiguration("dispenser_service_prefix"), + "tcp_name": LaunchConfiguration("dispenser_tcp_name"), + "require_tcp_for_taught_posx": ParameterValue( + LaunchConfiguration("dispenser_require_tcp_for_taught_posx"), + value_type=bool, + ), + "joint_velocity": ParameterValue( + LaunchConfiguration("dispenser_joint_velocity"), value_type=float + ), + "joint_acceleration": ParameterValue( + LaunchConfiguration("dispenser_joint_acceleration"), value_type=float + ), + "line_velocity": ParameterValue( + LaunchConfiguration("dispenser_line_velocity"), value_type=float + ), + "line_acceleration": ParameterValue( + LaunchConfiguration("dispenser_line_acceleration"), value_type=float + ), + } + ], + 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", 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( + package="azas_voice", + executable="tts_node", + name="tts_node", + output="screen", + parameters=[ + { + "enable_audio": ParameterValue(enable_tts_audio, value_type=bool), + "speech_rate": ParameterValue(tts_speech_rate, value_type=float), + "startup_prompt": tts_startup_prompt, + } + ], + 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, + "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/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 b77d21b..89a3a81 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, @@ -22,9 +23,14 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ + "conversation_manager_node = azas_voice.conversation_manager_node:main", "llm_recipe_mapper_node = azas_voice.llm_recipe_mapper_node:main", "recipe_mapper_node = azas_voice.recipe_mapper_node:main", "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 af5adb9..e63eccb 100644 --- a/src/azas_voice/test/test_command_parser.py +++ b/src/azas_voice/test/test_command_parser.py @@ -1,46 +1,174 @@ from azas_voice.command_parser import normalize_text, parse_recipe_command +from azas_voice.recipe_catalog import RECIPE_DISPENSERS, build_public_catalog -def assert_fixed_dispenser_ids(dispenser_ids): +def assert_dispenser_colors(dispenser_ids): assert dispenser_ids - assert all(item in {"1", "2", "3", "4"} for item in dispenser_ids) + assert all(item in {"red", "yellow", "green", "blue"} for item in dispenser_ids) def test_normalize_text_removes_spaces_and_lowercases(): assert normalize_text(" Recipe 1 ") == "recipe1" -def test_color_selection_maps_to_symbolic_dispenser_ids(): +def test_color_selection_maps_to_dispenser_colors(): decision = parse_recipe_command("노란색 파란색으로 만들어줘") assert decision.valid assert decision.intent == "make_cocktail" assert decision.recipe_id == "custom_color_selection" - assert decision.dispenser_ids == ("1", "3") + assert decision.dispenser_ids == ("yellow", "blue") -def test_number_selection_maps_to_fixed_dispenser_ids(): +def test_number_selection_maps_to_dispenser_colors(): decision = parse_recipe_command("디스펜서 2번 4번으로 만들어줘") assert decision.valid assert decision.intent == "make_cocktail" assert decision.recipe_id == "custom_color_selection" - assert decision.dispenser_ids == ("2", "4") + assert decision.dispenser_ids == ("yellow", "blue") def test_recipe_alias_maps_to_recipe_id(): decision = parse_recipe_command("3번 칵테일 만들어줘") assert decision.valid assert decision.recipe_id == "recipe_03" - assert_fixed_dispenser_ids(decision.dispenser_ids) + assert_dispenser_colors(decision.dispenser_ids) -def test_mood_request_randomly_recommends_executable_recipe(): +def test_four_menu_recipes_map_to_one_dispenser_each(): + expected = { + "1번 메뉴 만들어줘": ("recipe_01", ("red",)), + "2번 메뉴 만들어줘": ("recipe_02", ("yellow",)), + "3번 메뉴 만들어줘": ("recipe_03", ("green",)), + "4번 메뉴 만들어줘": ("recipe_04", ("blue",)), + "파란색 메뉴 만들어줘": ("recipe_04", ("blue",)), + } + + for utterance, (recipe_id, dispenser_ids) in expected.items(): + decision = parse_recipe_command(utterance) + assert decision.valid + assert decision.recipe_id == recipe_id + 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 assert decision.intent == "make_cocktail" - assert decision.recipe_id is not None - assert decision.recipe_id.startswith("recipe_") - assert_fixed_dispenser_ids(decision.dispenser_ids) + 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_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 + 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(): @@ -53,3 +181,65 @@ def test_cancel_intent(): decision = parse_recipe_command("취소해줘") assert decision.valid 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 + 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_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 + 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 d5d4326..618c0b1 100644 --- a/src/azas_voice/test/test_llm_recipe_mapper.py +++ b/src/azas_voice/test/test_llm_recipe_mapper.py @@ -1,7 +1,8 @@ from azas_voice.llm_recipe_mapper_node import _sanitize_llm_decision +from azas_voice.recipe_catalog import RECIPE_DISPENSERS -def test_sanitize_llm_decision_accepts_fixed_dispenser_numbers(): +def test_sanitize_llm_decision_converts_dispenser_numbers_to_colors(): decision = _sanitize_llm_decision( "2번 4번으로 만들어줘", { @@ -14,10 +15,10 @@ def test_sanitize_llm_decision_accepts_fixed_dispenser_numbers(): assert decision.valid assert decision.intent == "make_cocktail" - assert decision.dispenser_ids == ("2", "4") + assert decision.dispenser_ids == ("yellow", "blue") -def test_sanitize_llm_decision_converts_color_aliases_to_numbers(): +def test_sanitize_llm_decision_accepts_color_aliases(): decision = _sanitize_llm_decision( "노란색 파란색으로 만들어줘", { @@ -29,7 +30,7 @@ def test_sanitize_llm_decision_converts_color_aliases_to_numbers(): ) assert decision.valid - assert decision.dispenser_ids == ("1", "3") + assert decision.dispenser_ids == ("yellow", "blue") def test_sanitize_llm_decision_rejects_coordinate_like_output(): @@ -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,247 @@ 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_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( + "추천해줘", + { + "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 + 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(): + 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_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( + "진행해줘", + { + "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": "적게", + } diff --git a/src/azas_voice/test/test_tts_node.py b/src/azas_voice/test/test_tts_node.py new file mode 100644 index 0000000..3c7a523 --- /dev/null +++ b/src/azas_voice/test/test_tts_node.py @@ -0,0 +1,13 @@ +import json + +from azas_voice.tts_node import build_ui_state + + +def test_build_ui_state_preserves_korean_text(): + payload = json.loads(build_ui_state("speaking", "레드 메뉴를 만들까요?", "friendly")) + + assert payload == { + "state": "speaking", + "emotion": "friendly", + "text": "레드 메뉴를 만들까요?", + } diff --git a/src/azas_voice/test/test_voice_dispenser_executor_node.py b/src/azas_voice/test/test_voice_dispenser_executor_node.py new file mode 100644 index 0000000..3b00d56 --- /dev/null +++ b/src/azas_voice/test/test_voice_dispenser_executor_node.py @@ -0,0 +1,99 @@ +from azas_voice.voice_dispenser_executor_node import ( + build_dispenser_launch_command, + requests_from_decision, +) + + +def test_requests_from_confirmed_decision_repeats_amounts(): + decision = { + "confirmed": True, + "intent": "make_cocktail", + "recipe_id": "custom_preference_mix", + "dispenser_ids": ["red", "yellow", "blue"], + "dispenser_amounts": {"red": 2, "yellow": 0, "blue": 3}, + } + + requests = requests_from_decision(decision) + + assert [request.target_dispenser for request in requests] == [ + "red", + "red", + "blue", + "blue", + "blue", + ] + assert requests[0].repeat_index == 1 + assert requests[1].repeat_index == 2 + assert requests[1].repeat_total == 2 + + +def test_requests_from_plain_color_selection_defaults_to_one_press_each(): + decision = { + "confirmed": True, + "intent": "make_cocktail", + "recipe_id": "custom_color_selection", + "dispenser_ids": ["yellow", "blue"], + } + + requests = requests_from_decision(decision) + + assert [request.target_dispenser for request in requests] == ["yellow", "blue"] + assert all(request.repeat_total == 1 for request in requests) + + +def test_build_dispenser_launch_command_passes_target_and_safety_args(): + request = requests_from_decision( + { + "intent": "make_cocktail", + "recipe_id": "recipe_01", + "dispenser_ids": ["red"], + } + )[0] + + command = build_dispenser_launch_command( + request, + launch_file="dispenser_press.launch.py", + service_prefix="/", + tcp_name="rg2_tcp", + restore_tcp_after_run=True, + require_tcp_for_taught_posx=True, + allow_tcp_set_failure=False, + joint_velocity=10.0, + joint_acceleration=10.0, + line_velocity=15.0, + line_acceleration=25.0, + ) + + assert command[:4] == ["ros2", "launch", "azas_dispenser", "dispenser_press.launch.py"] + assert "target_dispenser:=red" in command + assert "service_prefix:=/" in command + assert "tcp_name:=rg2_tcp" in command + assert "line_velocity:=15.0" in command + + +def test_build_dispenser_launch_command_omits_empty_tcp_name(): + request = requests_from_decision( + { + "intent": "make_cocktail", + "recipe_id": "recipe_01", + "dispenser_ids": ["red"], + } + )[0] + + command = build_dispenser_launch_command( + request, + launch_file="dispenser_press.launch.py", + service_prefix="/", + tcp_name="", + restore_tcp_after_run=True, + require_tcp_for_taught_posx=False, + allow_tcp_set_failure=False, + joint_velocity=10.0, + joint_acceleration=10.0, + line_velocity=15.0, + line_acceleration=25.0, + ) + + # An empty tcp_name must not produce a malformed "tcp_name:=" launch arg. + assert not any(arg.startswith("tcp_name:=") for arg in command) + assert "require_tcp_for_taught_posx:=false" in command 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..2a81644 --- /dev/null +++ b/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py @@ -0,0 +1,39 @@ +from azas_voice.voice_pipeline_executor_node import ( + load_resume_snapshot, + recipe_colors_from_resume_snapshot, + stage_from_line, +) + + +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"} + + +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 new file mode 100644 index 0000000..87e9632 --- /dev/null +++ b/src/azas_voice/test/test_voice_screen_node.py @@ -0,0 +1,73 @@ +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(): + 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": "진행할까요?"} + + +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 new file mode 100644 index 0000000..ba85a32 --- /dev/null +++ b/src/azas_voice/web/voice.css @@ -0,0 +1,971 @@ +: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; +} + +.app-shell { + display: grid; + 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-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); + font-size: 13px; + 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; + 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; + 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); + } +} + +@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) { + .app-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; + } + + .drink-stats { + 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..4500dc0 --- /dev/null +++ b/src/azas_voice/web/voice.html @@ -0,0 +1,179 @@ + + + + + + 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..c8fb599 --- /dev/null +++ b/src/azas_voice/web/voice.js @@ -0,0 +1,678 @@ +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"); +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 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" }, + 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, + "컵 픽업 (쓰러진 컵)": 1, + "디스펜서 레시피 진행": 2, + "중단 지점 복구": 2, + "뚜껑 체결 / 쉐이킹": 3, + "쉐이킹": 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 = {}; + 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; + } 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); + step.classList.toggle("active", activeIndex === index); + }); + 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"; + robotStatusText.textContent = "주문 대기"; + return; + } + const status = pipeline.status || ""; + if (status === "failed" || status === "blocked") { + robotScene.dataset.step = "idle"; + robotStatusText.textContent = pipelineFailureText(pipeline); + return; + } + if (status === "completed" || activeIndex >= pipelineSteps.length) { + robotScene.dataset.step = "done"; + robotStatusText.textContent = "완료"; + return; + } + 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)]; +} + +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); + 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; + } + + 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" || 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") { + setBadge("making", "리허설 (dry run)"); + } else if (confirmedDecision.confirmed) { + setBadge("confirmed", "주문 확정"); + } else { + setBadge("recommended", "추천 메뉴 · \"응\" 하시면 시작해요"); + } +} + +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; +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 ? "확정됨" : "대기"; + renderMenu(state); + renderVisionCamera(state); +} + +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); + } +}); + +visionCameraImage.addEventListener("error", () => { + visionCameraImage.hidden = true; + visionCameraEmpty.hidden = false; + if (currentCameraStream) { + visionCameraEmpty.textContent = `${currentCameraStream} 프레임 대기`; + } +}); + +drawWaveform(); +refreshState().catch(() => {}); +setInterval(() => { + refreshState().catch(() => {}); +}, 500); 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 f75b14e..5b21e65 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -9,15 +9,15 @@ import cv2 import numpy as np import rclpy +import yaml from ament_index_python.packages import get_package_share_directory -from cv_bridge import CvBridge from geometry_msgs.msg import Pose, PoseStamped from rcl_interfaces.msg import ParameterDescriptor from moveit.core.robot_state import RobotState from moveit.planning import MoveItPy, PlanRequestParameters from moveit_msgs.msg import CollisionObject from rclpy.node import Node -from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data from scipy.spatial.transform import Rotation from sensor_msgs.msg import CameraInfo, Image, JointState from shape_msgs.msg import SolidPrimitive @@ -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), @@ -48,6 +49,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 @@ -81,6 +93,18 @@ 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 + 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 + score: float = 0.0 def clamp_to_safe_workspace(x, y, z, logger, z_min=SAFE_Z_MIN, clamp_xy=True): @@ -141,6 +165,22 @@ def parameter_array_or_empty(value): return list(value) +def parse_workspace_bounds(value): + if not isinstance(value, dict): + return None + required_keys = ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max") + if any(key not in value for key in required_keys): + return None + bounds = {key: float(value[key]) for key in required_keys} + if bounds["x_min"] > bounds["x_max"]: + raise ValueError("workspace_bounds_m x_min must be <= x_max") + if bounds["y_min"] > bounds["y_max"]: + raise ValueError("workspace_bounds_m y_min must be <= y_max") + if bounds["z_min"] > bounds["z_max"]: + raise ValueError("workspace_bounds_m z_min must be <= z_max") + return bounds + + def quat_dict_from_matrix(matrix): qx, qy, qz, qw = Rotation.from_matrix(matrix).as_quat() return { @@ -165,15 +205,21 @@ def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): } -def get_ee_matrix(moveit_robot): +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: - 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) @@ -191,7 +237,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("pick_depth_ratio", 0.55) + 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) @@ -199,8 +245,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) @@ -208,19 +262,35 @@ 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_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) 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("table_collision_enabled", False) + 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) self.declare_parameter("table_thickness", 0.04) @@ -229,6 +299,16 @@ def __init__(self): self.declare_parameter("table_center_x", 0.45) self.declare_parameter("table_center_y", 0.0) self.declare_parameter("table_publish_repeats", 3) + self.declare_parameter("table_collision_expand_to_workspace_walls", True) + self.declare_parameter( + "safety_config_path", + "/home/ssu/Azas/src/azas_bringup/config/safety.yaml", + ) + self.declare_parameter("safety_workspace_enforced", True) + self.declare_parameter("workspace_boundary_collision_enabled", True) + self.declare_parameter("workspace_boundary_collision_prefix", "side_grip_workspace") + self.declare_parameter("workspace_boundary_wall_thickness", 0.04) + self.declare_parameter("workspace_boundary_wall_clearance", 0.02) self.declare_parameter("gripper_open_settle_sec", 1.0) self.declare_parameter("pre_pick_joint1_clearance_deg", 12.0) self.declare_parameter( @@ -312,13 +392,14 @@ def __init__(self): self.declare_parameter("pick_z_offset", 0.20) self.declare_parameter("approach_offset", 0.12) self.declare_parameter("safe_z", 0.50) - self.declare_parameter("min_motion_z", 0.12) + self.declare_parameter("min_motion_z", 0.07) self.declare_parameter("workspace_xy_clamp_enabled", False) self.declare_parameter("return_home_after_task", True) self.declare_parameter("verify_motion", True) 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") @@ -346,7 +427,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.pick_depth_ratio = float(self.get_parameter("pick_depth_ratio").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 @@ -358,7 +439,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 ) @@ -376,6 +487,18 @@ 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_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 @@ -390,11 +513,14 @@ 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 ) + self.side_candidate_plan_check_enabled = parse_bool( + self.get_parameter("side_candidate_plan_check_enabled").value + ) self.side_linear_approach_enabled = parse_bool( self.get_parameter("side_linear_approach_enabled").value ) @@ -410,6 +536,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 ) @@ -427,6 +596,29 @@ def __init__(self): self.table_publish_repeats = max( 1, int(self.get_parameter("table_publish_repeats").value) ) + self.table_collision_expand_to_workspace_walls = parse_bool( + self.get_parameter("table_collision_expand_to_workspace_walls").value + ) + self.safety_config_path = str( + self.get_parameter("safety_config_path").value + ).strip() + self.safety_workspace_enforced = parse_bool( + self.get_parameter("safety_workspace_enforced").value + ) + self.workspace_boundary_collision_enabled = parse_bool( + self.get_parameter("workspace_boundary_collision_enabled").value + ) + self.workspace_boundary_collision_prefix = str( + self.get_parameter("workspace_boundary_collision_prefix").value + ).strip() + self.workspace_boundary_wall_thickness = max( + 0.001, + float(self.get_parameter("workspace_boundary_wall_thickness").value), + ) + self.workspace_boundary_wall_clearance = max( + 0.0, + float(self.get_parameter("workspace_boundary_wall_clearance").value), + ) self.gripper_open_settle_sec = max( 0.0, float(self.get_parameter("gripper_open_settle_sec").value) ) @@ -484,6 +676,7 @@ def __init__(self): self.workspace_xy_clamp_enabled = parse_bool( self.get_parameter("workspace_xy_clamp_enabled").value ) + self.workspace_bounds_m = self.load_safety_workspace_bounds() self.return_home_after_task = parse_bool( self.get_parameter("return_home_after_task").value ) @@ -497,6 +690,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 ) @@ -543,8 +739,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( @@ -567,6 +773,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 @@ -579,7 +792,42 @@ 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_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 " + 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( @@ -587,11 +835,11 @@ def __init__(self): "and will not avoid robot-link/table collisions." ) - self.bridge = CvBridge() self.color_image = None 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 @@ -606,13 +854,51 @@ def __init__(self): self.gripper2cam[:3, 3] /= 1000.0 self.get_logger().info(f"Loaded hand-eye calibration: {calib_file}") + self.gripper = None + self.robot = None + self.arm = None + self.robot_model = None + self.ompl_params = None + self.pilz_params = None + self.pilz_lin_params = None + self.collision_object_pub = None + self._motion_stack_ready = False + + self.home_ori = DOWN_ORI + + self.create_subscription( + CameraInfo, + "/camera/camera/color/camera_info", + self._camera_info_callback, + 10, + ) + self.create_subscription( + Image, + "/camera/camera/color/image_raw", + self._color_callback, + qos_profile_sensor_data, + ) + self.create_subscription( + Image, + "/camera/camera/aligned_depth_to_color/image_raw", + self._depth_callback, + qos_profile_sensor_data, + ) + + def ensure_motion_stack_ready(self): + if self._motion_stack_ready: + return True + log = self.get_logger() + + log.info("Initializing RG2 gripper client...") self.gripper = RG(GRIPPER_NAME, TOOLCHARGER_IP, TOOLCHARGER_PORT) + log.info("RG2 gripper client initialized") - self.get_logger().info("Initializing MoveItPy...") + log.info("Initializing MoveItPy...") self.robot = MoveItPy(node_name="yolo_cup_pick_moveit_py") self.arm = self.robot.get_planning_component(GROUP_NAME) self.robot_model = self.robot.get_robot_model() - self.get_logger().info("MoveItPy initialized") + log.info("MoveItPy initialized") self.collision_object_pub = self.create_publisher( CollisionObject, @@ -625,6 +911,7 @@ def __init__(self): ), ) self.publish_table_collision_if_enabled() + self.publish_workspace_boundary_collision_if_enabled() self.ompl_params = PlanRequestParameters(self.robot) self.ompl_params.planning_pipeline = "ompl" @@ -647,52 +934,277 @@ def __init__(self): self.pilz_lin_params.max_acceleration_scaling_factor = 0.05 self.pilz_lin_params.planning_time = 3.0 - self.home_ori = DOWN_ORI + self._motion_stack_ready = True + return True - self.create_subscription( - CameraInfo, - "/camera/camera/color/camera_info", - self._camera_info_callback, - 10, - ) - self.create_subscription( - Image, - "/camera/camera/color/image_raw", - self._color_callback, - 10, - ) - self.create_subscription( - Image, - "/camera/camera/aligned_depth_to_color/image_raw", - self._depth_callback, - 10, + def load_safety_workspace_bounds(self): + log = self.get_logger() + if not self.safety_workspace_enforced: + log.warning("safety_workspace_enforced=false: YAML workspace bounds are not enforced") + return None + if not self.safety_config_path: + raise ValueError("safety_workspace_enforced=true but safety_config_path is empty") + + safety_path = Path(self.safety_config_path).expanduser() + if not safety_path.exists(): + raise FileNotFoundError(f"safety config does not exist: {safety_path}") + with safety_path.open("r", encoding="utf-8") as stream: + safety_config = yaml.safe_load(stream) or {} + motion_config = safety_config.get("motion", {}) + if not isinstance(motion_config, dict): + raise ValueError(f"motion section is not a YAML map: {safety_path}") + + bounds = parse_workspace_bounds(motion_config.get("workspace_bounds_m")) + if bounds is None: + raise ValueError( + f"motion.workspace_bounds_m must contain x/y/z min/max values: {safety_path}" + ) + + min_z = motion_config.get("min_z_m") + if min_z is not None: + min_z = float(min_z) + if abs(min_z - bounds["z_min"]) > 1e-9: + log.warning( + f"safety min_z_m={min_z:.3f} differs from workspace z_min=" + f"{bounds['z_min']:.3f}; using the stricter/higher value" + ) + bounds["z_min"] = max(bounds["z_min"], min_z) + effective_min_z = max(self.min_motion_z, bounds["z_min"]) + if self.min_motion_z != effective_min_z: + log.info( + f"min_motion_z {self.min_motion_z:.3f} -> {effective_min_z:.3f} " + "from safety workspace" + ) + self.min_motion_z = effective_min_z + bounds["z_min"] = effective_min_z + + log.info( + "Loaded enforced safety workspace from " + f"{safety_path}: x=[{bounds['x_min']:.3f}, {bounds['x_max']:.3f}], " + f"y=[{bounds['y_min']:.3f}, {bounds['y_max']:.3f}], " + f"z=[{bounds['z_min']:.3f}, {bounds['z_max']:.3f}]" ) + return bounds - def publish_table_collision_if_enabled(self): - if not self.table_collision_enabled: - return + def validate_workspace_goal(self, x, y, z, label="pose goal"): + if not self.safety_workspace_enforced or self.workspace_bounds_m is None: + return True + bounds = self.workspace_bounds_m + violations = [] + if x < bounds["x_min"] or x > bounds["x_max"]: + violations.append( + f"x={x:.3f} outside [{bounds['x_min']:.3f}, {bounds['x_max']:.3f}]" + ) + if y < bounds["y_min"] or y > bounds["y_max"]: + violations.append( + f"y={y:.3f} outside [{bounds['y_min']:.3f}, {bounds['y_max']:.3f}]" + ) + if z < bounds["z_min"] or z > bounds["z_max"]: + violations.append( + f"z={z:.3f} outside [{bounds['z_min']:.3f}, {bounds['z_max']:.3f}]" + ) + if violations: + self.get_logger().error( + f"{label} rejected by safety workspace: " + "; ".join(violations) + ) + return False + return True - table = CollisionObject() - table.id = self.table_collision_id or "side_grip_table" - table.header.frame_id = BASE_FRAME + def make_box_collision_object(self, object_id, center_xyz, size_xyz): + collision_object = CollisionObject() + collision_object.id = object_id + collision_object.header.frame_id = BASE_FRAME primitive = SolidPrimitive() primitive.type = SolidPrimitive.BOX - primitive.dimensions = [ - self.table_size_x, - self.table_size_y, - self.table_thickness, - ] + primitive.dimensions = [float(value) for value in size_xyz] pose = Pose() - pose.position.x = self.table_center_x - pose.position.y = self.table_center_y - pose.position.z = self.table_surface_z - self.table_thickness * 0.5 + 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 - table.primitives.append(primitive) - table.primitive_poses.append(pose) - table.operation = CollisionObject.ADD + collision_object.primitives.append(primitive) + collision_object.primitive_poses.append(pose) + 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 + + table_center_x = self.table_center_x + table_center_y = self.table_center_y + table_size_x = self.table_size_x + table_size_y = self.table_size_y + if ( + self.table_collision_expand_to_workspace_walls + and self.workspace_bounds_m is not None + ): + clearance = ( + self.workspace_boundary_wall_clearance + if self.workspace_boundary_collision_enabled + else 0.0 + ) + x_min = self.workspace_bounds_m["x_min"] - clearance + x_max = self.workspace_bounds_m["x_max"] + clearance + y_min = self.workspace_bounds_m["y_min"] - clearance + y_max = self.workspace_bounds_m["y_max"] + clearance + table_center_x = (x_min + x_max) * 0.5 + table_center_y = (y_min + y_max) * 0.5 + table_size_x = x_max - x_min + table_size_y = y_max - y_min + + center_z = self.table_surface_z - self.table_thickness * 0.5 + table = self.make_box_collision_object( + self.table_collision_id or "side_grip_table", + [table_center_x, table_center_y, center_z], + [table_size_x, table_size_y, self.table_thickness], + ) for _ in range(self.table_publish_repeats): self.collision_object_pub.publish(table) @@ -702,10 +1214,76 @@ def publish_table_collision_if_enabled(self): "Added table collision object " f"id={table.id!r}, frame={BASE_FRAME}, " f"surface_z={self.table_surface_z:.3f}, " - f"size=({self.table_size_x:.3f}, {self.table_size_y:.3f}, " + f"size=({table_size_x:.3f}, {table_size_y:.3f}, " f"{self.table_thickness:.3f}), " - f"center=({self.table_center_x:.3f}, {self.table_center_y:.3f}, " - f"{pose.position.z:.3f})" + f"center=({table_center_x:.3f}, {table_center_y:.3f}, " + f"{center_z:.3f}), " + f"expanded_to_workspace_walls={self.table_collision_expand_to_workspace_walls}" + ) + + def publish_workspace_boundary_collision_if_enabled(self): + if not self.workspace_boundary_collision_enabled: + return + if not self.safety_workspace_enforced or self.workspace_bounds_m is None: + self.get_logger().warning( + "workspace_boundary_collision_enabled=true but safety workspace " + "is not enforced/loaded; skipping boundary collision objects" + ) + return + + bounds = self.workspace_bounds_m + thickness = self.workspace_boundary_wall_thickness + clearance = self.workspace_boundary_wall_clearance + prefix = self.workspace_boundary_collision_prefix or "side_grip_workspace" + + x_min = bounds["x_min"] - clearance + x_max = bounds["x_max"] + clearance + y_min = bounds["y_min"] - clearance + y_max = bounds["y_max"] + clearance + z_min = bounds["z_min"] + z_max = bounds["z_max"] + x_span = x_max - x_min + y_span = y_max - y_min + z_span = z_max - z_min + x_mid = (x_min + x_max) * 0.5 + y_mid = (y_min + y_max) * 0.5 + z_mid = (z_min + z_max) * 0.5 + + objects = [ + self.make_box_collision_object( + f"{prefix}_x_min_wall", + [x_min - thickness * 0.5, y_mid, z_mid], + [thickness, y_span + 2.0 * thickness, z_span], + ), + self.make_box_collision_object( + f"{prefix}_x_max_wall", + [x_max + thickness * 0.5, y_mid, z_mid], + [thickness, y_span + 2.0 * thickness, z_span], + ), + self.make_box_collision_object( + f"{prefix}_y_min_wall", + [x_mid, y_min - thickness * 0.5, z_mid], + [x_span + 2.0 * thickness, thickness, z_span], + ), + self.make_box_collision_object( + f"{prefix}_y_max_wall", + [x_mid, y_max + thickness * 0.5, z_mid], + [x_span + 2.0 * thickness, thickness, z_span], + ), + ] + + for _ in range(self.table_publish_repeats): + for collision_object in objects: + self.collision_object_pub.publish(collision_object) + time.sleep(0.05) + + self.get_logger().info( + "Added workspace boundary collision objects " + f"prefix={prefix!r}, thickness={thickness:.3f}, " + f"clearance={clearance:.3f}, " + f"x=[{x_min:.3f}, {x_max:.3f}], " + f"y=[{y_min:.3f}, {y_max:.3f}], " + f"z=[{z_min:.3f}, {z_max:.3f}]" ) def center_check_redetect(self, initial_base_xyz): @@ -817,7 +1395,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, @@ -835,12 +1413,34 @@ def _camera_info_callback(self, msg): } def _color_callback(self, msg): - self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + self.color_image = self.image_msg_to_bgr(msg) def _depth_callback(self, msg): - self.depth_image = self.bridge.imgmsg_to_cv2( - msg, desired_encoding="passthrough" - ) + self.depth_image = self.depth_msg_to_array(msg) + + def image_msg_to_bgr(self, msg): + 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)) + if enc == "rgba8": + return cv2.cvtColor(image, cv2.COLOR_RGBA2BGR) + return 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 color image encoding: {msg.encoding}") + + def depth_msg_to_array(self, msg): + enc = (msg.encoding or "").lower() + if enc in {"16uc1", "mono16"}: + return np.frombuffer(msg.data, dtype=np.uint16).reshape((msg.height, msg.width)) + if enc == "32fc1": + return np.frombuffer(msg.data, dtype=np.float32).reshape((msg.height, msg.width)) + raise RuntimeError(f"unsupported depth image encoding: {msg.encoding}") def plan_and_execute( self, @@ -854,11 +1454,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 @@ -877,12 +1477,14 @@ def plan_and_execute( pose_goal.pose.position.x = x pose_goal.pose.position.y = y pose_goal.pose.position.z = z + if not self.validate_workspace_goal(x, y, z, "pose goal"): + return False goal_xyz = np.array([x, y, z], dtype=float) log.info( 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 " @@ -910,10 +1512,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 = ( @@ -957,6 +1559,43 @@ def plan_and_execute( return False return True + def can_plan_pose_goal(self, pose_goal, params=None, label="candidate"): + log = self.get_logger() + 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) + else: + log.warning( + f"{label}: could not seed MoveIt start state from /joint_states; " + "falling back to current state" + ) + self.arm.set_start_state_to_current_state() + + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + x, y, z = clamp_to_safe_workspace( + x, + y, + z, + log, + self.min_motion_z, + self.workspace_xy_clamp_enabled, + ) + pose_goal.pose.position.x = x + pose_goal.pose.position.y = y + pose_goal.pose.position.z = z + 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=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") + return False + log.info(f"{label}: plan-check OK") + return True + def joint_position_error(self, target, current): return abs(math.atan2(math.sin(target - current), math.cos(target - current))) @@ -998,7 +1637,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, @@ -1008,7 +1647,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 @@ -1020,7 +1659,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( @@ -1038,7 +1677,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 @@ -1082,7 +1721,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 @@ -1108,7 +1747,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, @@ -1169,13 +1808,16 @@ 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 return False def wait_until_gripper_idle(self, timeout_sec=GRIPPER_OPEN_TIMEOUT_SEC): + if self.gripper is None: + self.get_logger().error("Gripper client is not initialized") + return False log = self.get_logger() start_time = time.time() while time.time() - start_time < timeout_sec: @@ -1194,6 +1836,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)" @@ -1214,6 +1858,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 = [] @@ -1243,33 +1888,17 @@ def detect_objects(self, image): key=lambda det: det["conf"], ) + self.last_detections = detections return detections def depth_candidates_from_bbox(self, bbox): x1, y1, x2, y2 = bbox h, w = self.depth_image.shape[:2] - x_ratios = [0.50, 0.35, 0.65, 0.25, 0.75] - y_ratios = [ - self.pick_depth_ratio, - 0.45, - 0.35, - 0.65, - 0.25, - 0.75, - ] - - points = [] - seen = set() - for yr in y_ratios: - for xr in x_ratios: - u = int(x1 + xr * (x2 - x1)) - v = int(y1 + yr * (y2 - y1)) - u = max(0, min(w - 1, u)) - v = max(0, min(h - 1, v)) - if (u, v) not in seen: - points.append((u, v)) - seen.add((u, v)) - return points + u = int(round(0.5 * (float(x1) + float(x2)))) + v = int(round(0.5 * (float(y1) + float(y2)))) + u = max(0, min(w - 1, u)) + v = max(0, min(h - 1, v)) + return [(u, v)] def depth_patch_at(self, u, v): h, w = self.depth_image.shape[:2] @@ -1305,17 +1934,15 @@ def depth_from_bbox(self, bbox, log_reason=False): if not valid_samples: if log_reason: log.warning( - "No valid depth found inside target bbox. " + "No valid depth found at target bbox center. " "Try a larger depth_patch_radius or lower min_depth_valid_ratio." ) return None - # Prefer the closest valid surface in the bbox. Transparent cups often - # expose background/table depth, so closest valid depth is usually safer. - u, v, z_m, valid_ratio = min(valid_samples, key=lambda sample: sample[2]) + u, v, z_m, valid_ratio = valid_samples[0] if log_reason: log.info( - f"Depth sample selected at ({u}, {v}): " + f"Depth sample selected at bbox center ({u}, {v}): " f"{z_m:.3f} m, valid_ratio={valid_ratio:.2f}" ) return u, v, z_m @@ -1333,8 +1960,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): @@ -1348,8 +1975,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: @@ -1363,47 +1990,48 @@ 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_unit_vector(self, cup_base_xyz=None): - direction = ( - self.side_direction_for_cup(cup_base_xyz) - 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) + def side_direction_candidates(self, cup_base_xyz): + return [self.side_direction_for_cup(cup_base_xyz)] + + def side_unit_vector(self, cup_base_xyz=None, direction=None): + if direction is None: + direction = ( + self.side_direction_for_cup(cup_base_xyz) + if cup_base_xyz is not None + else self.side_grasp_direction + ) return np.array([0.0, direction], dtype=float) def side_grasp_orientation(self, side_vec): @@ -1447,10 +2075,58 @@ def side_grasp_orientation(self, side_vec): ) return quat_dict_from_matrix(base_from_tool) - def compute_side_grasp_plan(self, cup_base_xyz) -> SideGraspPlan: + def side_plan_score(self, plan: SideGraspPlan, current_xyz): + stage_goal = np.array( + [plan.stage_xy[0], plan.stage_xy[1], plan.lift_z], + dtype=float, + ) + distance_score = float(np.linalg.norm(stage_goal - current_xyz)) + y_values = [ + float(plan.stage_xy[1]), + float(plan.pre_xy[1]), + float(plan.guarded_grasp_xy[1]), + ] + y_violation = sum( + max(self.side_stage_y_min - y_value, 0.0, y_value - self.side_stage_y_max) + for y_value in y_values + ) + 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 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) - side_direction = self.side_direction_for_cup(cup_xyz) - side_vec = self.side_unit_vector(cup_xyz) + if side_direction is None: + side_direction = self.side_direction_for_cup(cup_xyz) + side_direction = 1.0 if float(side_direction) >= 0.0 else -1.0 + side_vec = self.side_unit_vector(cup_xyz, side_direction) side_ori = self.side_grasp_orientation(side_vec) cup_xy = cup_xyz[:2] stage_offset = ( @@ -1458,19 +2134,87 @@ def compute_side_grasp_plan(self, cup_base_xyz) -> SideGraspPlan: 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) 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, @@ -1486,21 +2230,60 @@ def compute_side_grasp_plan(self, cup_base_xyz) -> SideGraspPlan: 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, + 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, + ) + + def build_side_grasp_candidates(self, cup_base_xyz): + 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})" + ) + 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"tool_roll={self.side_tool_roll_deg:.1f}deg" + 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}), " 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}, " @@ -1510,6 +2293,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: @@ -1576,6 +2373,8 @@ def base_from_detection(self, detection, log_prefix): def pick_and_place(self, base_xyz): if self.grasp_mode == "side": task_ok = self.pick_and_place_side(base_xyz) + if task_ok: + return True else: task_ok = self.pick_and_place_top(base_xyz) @@ -1600,26 +2399,8 @@ def refine_target_from_current_view(self, log): return None return self.base_from_detection(target, "[redetect]") - def pick_and_place_side(self, base_xyz): + def execute_side_grasp_plan(self, plan: SideGraspPlan): log = self.get_logger() - initial_base = np.array([float(v) for v in base_xyz], dtype=float) - - refined_base = self.center_check_redetect(initial_base) - cup_base = initial_base if refined_base is None else np.array(refined_base, dtype=float) - plan = self.compute_side_grasp_plan(cup_base) - self.log_side_grasp_plan(plan) - - 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(plan.cup_xyz): - return False - if not self.move_joint1_clearance_before_side_grip(): - return False - side_close_xy = ( plan.pre_xy if self.side_final_slide_enabled else plan.guarded_grasp_xy ) @@ -1640,6 +2421,15 @@ def pick_and_place_side(self, base_xyz): ): 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 @@ -1667,6 +2457,13 @@ def pick_and_place_side(self, base_xyz): 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( @@ -1696,50 +2493,74 @@ def pick_and_place_side(self, base_xyz): "side_move_to_initial_center_before_close is disabled at runtime " "because moving to the cup center before close can push the cup." ) - close_xy = plan.guarded_grasp_xy.copy() - active_lift_z = max(active_pre_z + self.approach_offset, plan.lift_z) - active_place_z = max(active_pre_z, self.min_motion_z) - active_place_approach_z = max(active_place_z + self.approach_offset, self.safe_z) log.info("close gripper for guarded side grasp") self.gripper.move_gripper(GRIPPER_CLOSE_WIDTH, GRIPPER_FORCE) time.sleep(1.0) + log.info("side grasp complete; holding cup for downstream rule-based task") + return True - move_steps = [ - ( - "lift cup", - make_pose(close_xy[0], close_xy[1], active_lift_z, plan.orientation), - self.side_final_approach_params(), - ), - ( - "move above syrup pump front", - make_pose(self.place_x, self.place_y, active_place_approach_z, plan.orientation), - self.ompl_params, - ), - ( - "place cup", - make_pose(self.place_x, self.place_y, active_place_z, plan.orientation), - self.pilz_params, - ), - ] - for label, pose, params in move_steps: - log.info(label) - if not self.plan_and_execute(pose_goal=pose, params=params): + def pick_and_place_side(self, base_xyz): + log = self.get_logger() + initial_base = np.array([float(v) for v in base_xyz], dtype=float) + + refined_base = self.center_check_redetect(initial_base) + cup_base = initial_base if refined_base is None else np.array(refined_base, dtype=float) + 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 - log.info("open gripper") - self.open_gripper_max(wait=True) + 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})" + ) - log.info("retract") - return self.plan_and_execute( - pose_goal=make_pose( - self.place_x, - self.place_y, - plan.place_approach_z, - plan.orientation, - ), - params=self.pilz_params, - ) + 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, + ) + 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 + finally: + if cup_collision_added: + self.remove_side_cup_collision_if_enabled() def pick_and_place_top(self, base_xyz): log = self.get_logger() @@ -1836,6 +2657,11 @@ def start_pick_from_detection(self): self.last_status = f"no {self.target_class} detection" return + self.last_status = "initializing robot motion" + if not self.ensure_motion_stack_ready(): + self.last_status = "motion init failed" + return + self.last_status = f"pick requested: {self.target_class}" base_xyz = self.base_from_detection(self.last_detection, "[initial]") if base_xyz is None: @@ -1854,13 +2680,16 @@ def start_pick_from_detection(self): self.last_status = "pick finished" else: if self.auto_pick: - self.has_picked_once = True + self.last_pick_time = time.time() log.warning( - "auto_pick attempt failed; automatic retry is latched off until reset" + "auto_pick attempt failed; returning to camera home and retrying after interval" ) self.last_status = "pick failed" finally: - if self.return_to_camera_home_after_attempt: + if ( + self.return_to_camera_home_after_attempt + and not (task_ok and self.grasp_mode == "side") + ): log.info("return to camera home after pick attempt") if self.move_camera_home(): if task_ok: @@ -1945,7 +2774,12 @@ 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; opening preview before motion stack initialization") + elif self.move_to_camera_home: + if not self.ensure_motion_stack_ready(): + log.error("Motion stack initialization failed") + return if self.move_joint_home_before_camera_home: log.info("Move JOINT HOME before high camera home") if not self.move_joint_home(): @@ -1956,18 +2790,25 @@ def run(self): log.error("High camera home move failed") return else: + if not self.ensure_motion_stack_ready(): + log.error("Motion stack initialization failed") + return log.info("Move JOINT HOME") if not self.move_joint_home(): log.error("Joint home move failed") return - self.open_gripper_max(wait=True) + if self._motion_stack_ready: + self.open_gripper_max(wait=True) window = "YOLO Cup Pick - p pick, a auto, r reset, esc quit" cv2.namedWindow(window) 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 @@ -2008,7 +2849,8 @@ def run(self): def destroy_node(self): try: - self.gripper.close_connection() + if self.gripper is not None: + self.gripper.close_connection() finally: super().destroy_node() 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 eca98c9..3049843 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,17 @@ from copy import deepcopy from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, 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 from launch_ros.parameter_descriptions import ParameterValue @@ -16,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: { @@ -50,6 +70,80 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params ) ) + if _as_bool(LaunchConfiguration("workspace_collision_scene_enabled").perform(context)): + nodes.append( + Node( + package="azas_motion", + executable="workspace_collision_scene_node", + name="workspace_collision_scene_node", + output="screen", + parameters=[ + { + "safety_config_path": ParameterValue( + LaunchConfiguration("safety_config_path"), + value_type=str, + ), + "publish_period_sec": ParameterValue( + LaunchConfiguration( + "workspace_collision_publish_period_sec" + ), + value_type=float, + ), + "table_collision_enabled": ParameterValue( + LaunchConfiguration("table_collision_enabled"), + value_type=bool, + ), + "table_surface_z": ParameterValue( + LaunchConfiguration("table_surface_z"), + value_type=float, + ), + "table_thickness": ParameterValue( + LaunchConfiguration("table_thickness"), + value_type=float, + ), + "table_size_x": ParameterValue( + LaunchConfiguration("table_size_x"), + value_type=float, + ), + "table_size_y": ParameterValue( + LaunchConfiguration("table_size_y"), + value_type=float, + ), + "table_center_x": ParameterValue( + LaunchConfiguration("table_center_x"), + value_type=float, + ), + "table_center_y": ParameterValue( + LaunchConfiguration("table_center_y"), + value_type=float, + ), + "table_collision_expand_to_workspace_walls": ParameterValue( + LaunchConfiguration( + "table_collision_expand_to_workspace_walls" + ), + value_type=bool, + ), + "workspace_boundary_collision_enabled": ParameterValue( + LaunchConfiguration("workspace_boundary_collision_enabled"), + value_type=bool, + ), + "workspace_boundary_collision_prefix": ParameterValue( + LaunchConfiguration("workspace_boundary_collision_prefix"), + value_type=str, + ), + "workspace_boundary_wall_thickness": ParameterValue( + LaunchConfiguration("workspace_boundary_wall_thickness"), + value_type=float, + ), + "workspace_boundary_wall_clearance": ParameterValue( + LaunchConfiguration("workspace_boundary_wall_clearance"), + value_type=float, + ), + } + ], + ) + ) + if _as_bool(LaunchConfiguration("dispenser_collision_enabled").perform(context)): nodes.append( Node( @@ -78,15 +172,28 @@ 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, - { + 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")), + ) + ) + + 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, @@ -102,7 +209,7 @@ 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"), + "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"), @@ -113,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"), @@ -128,6 +260,15 @@ 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_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( @@ -145,6 +286,9 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "side_auto_direction_by_cup_y": LaunchConfiguration( "side_auto_direction_by_cup_y" ), + "side_candidate_plan_check_enabled": LaunchConfiguration( + "side_candidate_plan_check_enabled" + ), "side_linear_approach_enabled": LaunchConfiguration( "side_linear_approach_enabled" ), @@ -158,6 +302,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" ), @@ -167,6 +349,29 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "table_size_y": LaunchConfiguration("table_size_y"), "table_center_x": LaunchConfiguration("table_center_x"), "table_center_y": LaunchConfiguration("table_center_y"), + "table_collision_expand_to_workspace_walls": LaunchConfiguration( + "table_collision_expand_to_workspace_walls" + ), + "safety_config_path": ParameterValue( + LaunchConfiguration("safety_config_path"), + value_type=str, + ), + "safety_workspace_enforced": LaunchConfiguration( + "safety_workspace_enforced" + ), + "workspace_boundary_collision_enabled": LaunchConfiguration( + "workspace_boundary_collision_enabled" + ), + "workspace_boundary_collision_prefix": ParameterValue( + LaunchConfiguration("workspace_boundary_collision_prefix"), + value_type=str, + ), + "workspace_boundary_wall_thickness": LaunchConfiguration( + "workspace_boundary_wall_thickness" + ), + "workspace_boundary_wall_clearance": LaunchConfiguration( + "workspace_boundary_wall_clearance" + ), "side_orientation_mode": ParameterValue( LaunchConfiguration("side_orientation_mode"), value_type=str, @@ -197,6 +402,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" @@ -249,8 +457,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 @@ -294,8 +516,10 @@ 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" + 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" @@ -312,9 +536,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", @@ -350,6 +614,21 @@ 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_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" ) @@ -375,14 +654,19 @@ 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", default_value="false", description="If true, flip side direction by cup Y; disabled by default because the measured dispenser row is on the -Y side.", ) + side_candidate_plan_check_enabled_arg = DeclareLaunchArgument( + "side_candidate_plan_check_enabled", + default_value="true", + description="Plan-check both side-grip approach candidates before executing the first feasible one.", + ) side_linear_approach_enabled_arg = DeclareLaunchArgument( "side_linear_approach_enabled", default_value="true", @@ -395,7 +679,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( @@ -408,6 +692,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", @@ -443,11 +787,29 @@ 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="false", + description=( + "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( "table_collision_enabled", - default_value="false", + default_value="true", description="Publish a base_link table collision box so MoveIt avoids robot-link/table collisions.", ) + workspace_collision_scene_enabled_arg = DeclareLaunchArgument( + "workspace_collision_scene_enabled", + default_value="true", + description="Start the shared workspace collision scene node for table and boundary walls.", + ) + workspace_collision_publish_period_sec_arg = DeclareLaunchArgument( + "workspace_collision_publish_period_sec", + default_value="2.0", + description="Republish period for shared workspace collision objects.", + ) table_surface_z_arg = DeclareLaunchArgument( "table_surface_z", default_value="0.0", @@ -478,6 +840,47 @@ def generate_launch_description(): default_value="0.0", description="Table collision box center Y in base_link meters.", ) + table_collision_expand_to_workspace_walls_arg = DeclareLaunchArgument( + "table_collision_expand_to_workspace_walls", + default_value="true", + description="Expand table collision XY footprint to the workspace wall inner faces.", + ) + safety_config_path_arg = DeclareLaunchArgument( + "safety_config_path", + default_value=PathJoinSubstitution( + [ + FindPackageShare("azas_bringup"), + "config", + "safety.yaml", + ] + ), + description="YAML with enforced base_link workspace bounds.", + ) + safety_workspace_enforced_arg = DeclareLaunchArgument( + "safety_workspace_enforced", + default_value="true", + description="Fail closed before planning/execution when a pose goal is outside safety.yaml workspace bounds.", + ) + workspace_boundary_collision_enabled_arg = DeclareLaunchArgument( + "workspace_boundary_collision_enabled", + default_value="true", + description="Publish workspace wall collision objects from safety.yaml XY bounds.", + ) + workspace_boundary_collision_prefix_arg = DeclareLaunchArgument( + "workspace_boundary_collision_prefix", + default_value="side_grip_workspace", + description="Collision object ID prefix for workspace boundary walls.", + ) + workspace_boundary_wall_thickness_arg = DeclareLaunchArgument( + "workspace_boundary_wall_thickness", + default_value="0.04", + description="Thickness in meters for workspace boundary wall collision boxes.", + ) + workspace_boundary_wall_clearance_arg = DeclareLaunchArgument( + "workspace_boundary_wall_clearance", + default_value="0.02", + description="Extra XY clearance outside safety.yaml bounds for wall collision boxes; pose goals still use safety.yaml bounds.", + ) side_orientation_mode_arg = DeclareLaunchArgument( "side_orientation_mode", default_value="approach", @@ -543,6 +946,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" ) @@ -596,7 +1004,7 @@ def generate_launch_description(): ) min_motion_z_arg = DeclareLaunchArgument( "min_motion_z", - default_value="0.12", + default_value="0.07", description="Minimum allowed commanded Z in base frame.", ) workspace_xy_clamp_enabled_arg = DeclareLaunchArgument( @@ -626,6 +1034,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( [ @@ -635,7 +1058,7 @@ def generate_launch_description(): device_arg, target_class_arg, auto_pick_interval_arg, - pick_depth_ratio_arg, + exit_after_pick_arg, depth_patch_radius_arg, min_depth_valid_ratio_arg, min_depth_m_arg, @@ -643,7 +1066,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, @@ -651,6 +1082,9 @@ 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_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, @@ -658,16 +1092,32 @@ def generate_launch_description(): side_low_retry_lift_m_arg, side_low_retry_attempts_arg, side_auto_direction_by_cup_y_arg, + side_candidate_plan_check_enabled_arg, side_linear_approach_enabled_arg, side_final_slide_enabled_arg, 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, 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, table_surface_z_arg, table_thickness_arg, @@ -675,6 +1125,13 @@ def generate_launch_description(): table_size_y_arg, table_center_x_arg, table_center_y_arg, + table_collision_expand_to_workspace_walls_arg, + safety_config_path_arg, + safety_workspace_enforced_arg, + workspace_boundary_collision_enabled_arg, + workspace_boundary_collision_prefix_arg, + workspace_boundary_wall_thickness_arg, + workspace_boundary_wall_clearance_arg, side_orientation_mode_arg, side_tool_roll_deg_arg, side_roll_deg_arg, @@ -692,6 +1149,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, @@ -717,6 +1175,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/third_party/ros2_src/doosan-robot2 b/third_party/ros2_src/doosan-robot2 index 0a908f3..a759bea 160000 --- a/third_party/ros2_src/doosan-robot2 +++ b/third_party/ros2_src/doosan-robot2 @@ -1 +1 @@ -Subproject commit 0a908f31222e795e2e39ab5f187109ebbd85541b +Subproject commit a759bea7b789e968e82c82539518c676d06c9eaa 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 diff --git a/tools/checks/check_dispenser_color_scan.py b/tools/checks/check_dispenser_color_scan.py new file mode 100644 index 0000000..35fd520 --- /dev/null +++ b/tools/checks/check_dispenser_color_scan.py @@ -0,0 +1,143 @@ +#!/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"} +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: + 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 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" + 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}") + + 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 + + +if __name__ == "__main__": + raise SystemExit(main()) 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/checks/check_hand_detection_perception_only.py b/tools/checks/check_hand_detection_perception_only.py new file mode 100755 index 0000000..853f3ce --- /dev/null +++ b/tools/checks/check_hand_detection_perception_only.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +"""Static guard: the hand detection node must stay perception-only (no motion).""" +from __future__ import annotations + +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +NODE = ROOT / "tools" / "perception" / "human_hand_detection_node.py" + +FORBIDDEN = ( + "MoveLine", + "MoveJoint", + "MoveWait", + "motion/move", + "SetGripper", + "rg2", + "create_client", +) +REQUIRED = ( + "/azas/human_hand_detection", + "no_motion_hri_perception_only", + "PointStamped", +) + + +def main() -> int: + text = NODE.read_text(encoding="utf-8") + failures: list[str] = [] + for needle in FORBIDDEN: + if needle in text: + failures.append(f"forbidden motion-related token present: {needle!r}") + for needle in REQUIRED: + if needle not in text: + failures.append(f"required token missing: {needle!r}") + if failures: + for failure in failures: + print(f"[FAIL] {NODE.relative_to(ROOT)}: {failure}") + return 1 + print("[PASS] human hand detection node is perception-only (no motion clients, required topics present)") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) 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/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/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/checks/check_measured_dispenser_geometry.py b/tools/checks/check_measured_dispenser_geometry.py index 23e8a00..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.43, 0.18, 0.392], - "2": [0.43, 0.08, 0.392], - "3": [0.43, -0.02, 0.392], - "4": [0.43, -0.12, 0.392], + "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.51, 0.18, 0.367], - "2": [0.51, 0.08, 0.367], - "3": [0.51, -0.02, 0.367], - "4": [0.51, -0.12, 0.367], + "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_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/checks/check_oss_stack.sh b/tools/checks/check_oss_stack.sh index f376b3d..ac78eb4 100755 --- a/tools/checks/check_oss_stack.sh +++ b/tools/checks/check_oss_stack.sh @@ -57,7 +57,7 @@ echo "[Azas] Checking ROS package availability" check "azas_bringup package" ros2 pkg prefix azas_bringup check "azas_perception package" ros2 pkg prefix azas_perception check "azas_interfaces package" ros2 pkg prefix azas_interfaces -check "jarvis package for floor-place/RG2 bridge" ros2 pkg prefix jarvis +check "azas_gripper package for RG2 bridge" ros2 pkg prefix azas_gripper check "Doosan bringup package" ros2 pkg prefix dsr_bringup2 check "Doosan M0609 MoveIt config package" ros2 pkg prefix dsr_moveit_config_m0609 check "MoveItPy package" ros2 pkg prefix moveit_py diff --git a/tools/checks/check_panel_gripper_reconnect_and_force.py b/tools/checks/check_panel_gripper_reconnect_and_force.py index fd86a70..9802390 100755 --- a/tools/checks/check_panel_gripper_reconnect_and_force.py +++ b/tools/checks/check_panel_gripper_reconnect_and_force.py @@ -22,6 +22,7 @@ def main() -> int: require(PANEL, "RG2_STACK_PATTERNS") require(PANEL, "elif step.key == \"connect_gripper\":") require(PANEL, "cleanup_rg2_stack()") + 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") diff --git a/tools/checks/check_panel_service_discovery_race.py b/tools/checks/check_panel_service_discovery_race.py index e04db92..89def56 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,149 @@ 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 = [ + "connect_robot", + "status_check", + "start_collision_scene", + "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: + 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 = "tools/run/run_changhyun_side_grip_direct.sh" + if expected_package_source not in side_grip_command: + 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 + 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 + 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} + + 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/checks/check_realsense_camera_ready.sh b/tools/checks/check_realsense_camera_ready.sh new file mode 100755 index 0000000..5ac060c --- /dev/null +++ b/tools/checks/check_realsense_camera_ready.sh @@ -0,0 +1,110 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")/../.." && pwd)" +ROS_SETUP="${ROS_SETUP:-/opt/ros/humble/setup.bash}" +INSTALL_SETUP="${INSTALL_SETUP:-$ROOT/install/local_setup.bash}" +TIMEOUT_SEC="${TIMEOUT_SEC:-20}" + +TOPICS=( + "/camera/camera/color/image_raw" + "/camera/camera/aligned_depth_to_color/image_raw" + "/camera/camera/color/camera_info" +) + +log() { + echo "[Azas camera] $*" +} + +check_topic_once() { + local topic="$1" + timeout 3s ros2 topic echo --once "$topic" >/tmp/azas_camera_topic_sample.txt 2>&1 +} + +wait_for_topics() { + local deadline=$((SECONDS + TIMEOUT_SEC)) + local missing=() + + while (( SECONDS < deadline )); do + missing=() + for topic in "${TOPICS[@]}"; do + if ! check_topic_once "$topic"; then + missing+=("$topic") + fi + done + + if ((${#missing[@]} == 0)); then + return 0 + fi + sleep 1 + done + + log "missing topics after ${TIMEOUT_SEC}s:" + printf ' - %s\n' "${missing[@]}" + log "last ros2 output:" + sed 's/^/ /' /tmp/azas_camera_topic_sample.txt 2>/dev/null || true + return 1 +} + +[[ -f "$ROS_SETUP" ]] || { + log "missing ROS setup: $ROS_SETUP" + exit 1 +} + +# shellcheck source=/opt/ros/humble/setup.bash +source "$ROS_SETUP" +if [[ -f "$INSTALL_SETUP" ]]; then + # shellcheck source=/dev/null + source "$INSTALL_SETUP" +fi + +if ! ros2 pkg prefix realsense2_camera >/dev/null 2>&1; then + log "missing ROS package: realsense2_camera" + log "install example: sudo apt install -y ros-humble-realsense2-camera" + exit 1 +fi +log "realsense2_camera package: $(ros2 pkg prefix realsense2_camera)" + +if command -v lsusb >/dev/null 2>&1; then + if lsusb | grep -Eiq "Intel|RealSense"; then + log "USB device candidate:" + lsusb | grep -Ei "Intel|RealSense" | sed 's/^/ /' + else + log "no Intel/RealSense USB device found by lsusb" + log "check USB3 cable/port, power, and whether another PC owns the camera" + fi +else + log "lsusb not found; skipping USB device listing" +fi + +video_count="$(find /dev -maxdepth 1 -name 'video*' 2>/dev/null | wc -l)" +log "/dev/video* count: $video_count" +if id -nG | tr ' ' '\n' | grep -qx video; then + log "current user is in video group" +else + log "current user is not in video group; camera access may fail until relogin after: sudo usermod -aG video $USER" +fi + +if wait_for_topics; then + log "camera topics already publishing" + exit 0 +fi + +log "starting temporary RealSense launch for readiness check" +ros2 launch realsense2_camera rs_launch.py \ + camera_name:=camera \ + enable_color:=true \ + enable_depth:=true \ + align_depth.enable:=true >/tmp/azas_realsense_launch.log 2>&1 & +camera_pid=$! +trap 'kill "$camera_pid" >/dev/null 2>&1 || true' EXIT + +if wait_for_topics; then + log "PASS: color, aligned depth, and camera_info topics are publishing" + exit 0 +fi + +log "RealSense launch log tail:" +tail -80 /tmp/azas_realsense_launch.log | sed 's/^/ /' +log "FAIL: camera is not ready for panel side_grip" +exit 1 diff --git a/tools/checks/check_rg2_moveit_description.py b/tools/checks/check_rg2_moveit_description.py new file mode 100755 index 0000000..11cbd91 --- /dev/null +++ b/tools/checks/check_rg2_moveit_description.py @@ -0,0 +1,160 @@ +#!/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" +) +MOVEIT_SRDF = MOVEIT_XACRO.with_name("dsr.srdf") + +REQUIRED_LINKS = { + "rg2_quick_changer", + "rg2_gripper_body", + "rg2_angle_bracket", + "rg2_left_inner_finger", + "rg2_right_inner_finger", + "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() + + +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="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: + 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 != "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=link_6") + print("rg2_mount_rpy=0 0 0") + print(f"rg2_required_disabled_collisions={len(REQUIRED_DISABLED_COLLISIONS)}") + 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/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/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py new file mode 100644 index 0000000..4c1c0df --- /dev/null +++ b/tools/perception/dispenser_color_scan.py @@ -0,0 +1,579 @@ +#!/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 itertools +import json +import math +import os +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 +# 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. +# 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),), +} +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: + """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: + 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 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) + 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 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 <= center_x <= 0.90 * img_w): + continue + score = area + float(h) * 10.0 + 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))) + 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]: + 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] = {} + 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( + *, + 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 + 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 + + frame_bgr = None + frame_count = 0 + first_frame_time: float | None = 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, 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 + 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 + 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 and frame_count >= max(sample_frames, 1): + 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) + + 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: + # 한 프레임만 보면 일시적 가림(작업자 팔 등)으로 핸들 하나가 빠져 + # 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: + 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: + 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: + 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) + 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 + 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") + 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: + 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( + 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: + out = Path(args.output) + failed_out = out.with_suffix(out.suffix + ".failed") + 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))), + 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) + 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 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/perception/human_hand_detection_node.py b/tools/perception/human_hand_detection_node.py new file mode 100755 index 0000000..2006471 --- /dev/null +++ b/tools/perception/human_hand_detection_node.py @@ -0,0 +1,307 @@ +#!/usr/bin/env python3 +"""Perception-only human hand detection for the post-shake handover plan. + +Publishes a stable open-hand 3D target on /azas/human_hand_detection as a +geometry_msgs/PointStamped in the color camera optical frame. This node sends +NO robot motion command of any kind; it only reads camera topics, following +docs/post_shake_human_handover_plan.md phase VERIFY_HUMAN_HAND_TRACKING +(gate: no_motion_hri_perception_only). + +Pipeline: + RealSense color + aligned depth -> MediaPipe HandLandmarker (tasks API) + -> open-palm heuristic over 21 landmarks -> palm-center pixel + -> median depth window -> intrinsics deprojection -> stability window + -> publish only while the hand stays open and spatially stable. + +Usage: + python3 tools/perception/human_hand_detection_node.py + python3 tools/perception/human_hand_detection_node.py --show-overlay false +""" +from __future__ import annotations + +import argparse +import collections +import json +import math +import time + +import cv2 +import numpy as np +import rclpy +from geometry_msgs.msg import PointStamped +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from std_msgs.msg import String + +import mediapipe as mp +from mediapipe.tasks import python as mp_python +from mediapipe.tasks.python import vision as mp_vision + +DEFAULT_MODEL_PATH = "/home/ssu/Azas/models/mediapipe/hand_landmarker.task" +COLOR_TOPIC = "/camera/camera/color/image_raw" +DEPTH_TOPIC = "/camera/camera/aligned_depth_to_color/image_raw" +CAMERA_INFO_TOPIC = "/camera/camera/color/camera_info" +OUTPUT_TOPIC = "/azas/human_hand_detection" +STATUS_TOPIC = "/azas/human_hand_detection/status" +OVERLAY_TOPIC = "/azas/human_hand_detection/overlay" + +WRIST = 0 +PALM_LANDMARKS = (0, 5, 9, 13, 17) +FINGER_TIPS = (8, 12, 16, 20) +FINGER_PIPS = (6, 10, 14, 18) + + +# cv_bridge is avoided on purpose: the ROS humble build is ABI-incompatible +# with the pip-installed numpy 2.x that mediapipe requires. +def image_msg_to_array(msg: Image) -> np.ndarray: + if msg.encoding in ("bgr8", "rgb8"): + array = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return cv2.cvtColor(array, cv2.COLOR_RGB2BGR) if msg.encoding == "rgb8" else array.copy() + if msg.encoding == "16UC1": + dtype = np.dtype(np.uint16).newbyteorder(">" if msg.is_bigendian else "<") + return np.frombuffer(msg.data, dtype=dtype).reshape(msg.height, msg.width) + if msg.encoding == "32FC1": + dtype = np.dtype(np.float32).newbyteorder(">" if msg.is_bigendian else "<") + return np.frombuffer(msg.data, dtype=dtype).reshape(msg.height, msg.width) + raise ValueError(f"unsupported image encoding: {msg.encoding}") + + +def bgr_array_to_image_msg(array: np.ndarray, header) -> Image: + msg = Image() + msg.header = header + msg.height, msg.width = array.shape[:2] + msg.encoding = "bgr8" + msg.is_bigendian = 0 + msg.step = msg.width * 3 + msg.data = np.ascontiguousarray(array).tobytes() + 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.""" + + def __init__(self, args: argparse.Namespace) -> None: + super().__init__("azas_human_hand_detection") + self.args = args + self.camera_info: CameraInfo | None = None + self.latest_depth: np.ndarray | None = None + self.latest_depth_encoding = "" + self.last_process_monotonic = 0.0 + self.last_timestamp_ms = 0 + # Recent accepted (monotonic_time, xyz_m) detections for the stability gate. + self.recent: collections.deque[tuple[float, tuple[float, float, float]]] = collections.deque(maxlen=64) + + options = mp_vision.HandLandmarkerOptions( + base_options=mp_python.BaseOptions(model_asset_path=args.model_path), + running_mode=mp_vision.RunningMode.VIDEO, + num_hands=1, + min_hand_detection_confidence=args.min_detection_confidence, + min_tracking_confidence=args.min_tracking_confidence, + ) + self.landmarker = mp_vision.HandLandmarker.create_from_options(options) + + self.point_pub = self.create_publisher(PointStamped, OUTPUT_TOPIC, 10) + self.status_pub = self.create_publisher(String, STATUS_TOPIC, 10) + self.overlay_pub = self.create_publisher(Image, OVERLAY_TOPIC, 2) if args.show_overlay else None + + self.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, self.on_camera_info, 10) + self.create_subscription(Image, DEPTH_TOPIC, self.on_depth, 5) + self.create_subscription(Image, COLOR_TOPIC, self.on_color, 5) + + self.get_logger().info( + "human hand detection ready (perception-only, no motion commands). " + f"publishing stable open-hand target on {OUTPUT_TOPIC}; " + f"stability: {args.stable_min_samples} samples within {args.stable_radius_m:.3f}m " + f"over >= {args.stable_min_seconds:.2f}s" + ) + + def on_camera_info(self, msg: CameraInfo) -> None: + self.camera_info = msg + + def on_depth(self, msg: Image) -> None: + self.latest_depth = image_msg_to_array(msg) + self.latest_depth_encoding = msg.encoding + + def on_color(self, msg: Image) -> None: + now = time.monotonic() + if now - self.last_process_monotonic < 1.0 / max(self.args.max_rate_hz, 0.5): + return + self.last_process_monotonic = now + if self.camera_info is None or self.latest_depth is None: + self.publish_status({"detected": False, "reason": "waiting for camera_info/depth"}) + return + + color = image_msg_to_array(msg) + 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.copy() if self.overlay_pub is not None else None + status: dict[str, object] = {"detected": False} + try: + if not result.hand_landmarks: + self.recent.clear() + status["reason"] = "no hand" + return + landmarks = result.hand_landmarks[0] + height, width = color.shape[:2] + 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 = ( + int(np.clip(np.mean([pixels[i][0] for i in PALM_LANDMARKS]), 0, width - 1)), + int(np.clip(np.mean([pixels[i][1] for i in PALM_LANDMARKS]), 0, height - 1)), + ) + depth_m = self.median_depth_m(palm_px) + status.update( + { + "detected": True, + "open_fingers": open_fingers, + "hand_open": hand_open, + "palm_px": list(palm_px), + "depth_m": None if depth_m is None else round(depth_m, 4), + } + ) + if overlay is not None: + for px, py in pixels: + cv2.circle(overlay, (int(px), int(py)), 3, (0, 255, 0) if hand_open else (0, 165, 255), -1) + cv2.circle(overlay, palm_px, 8, (255, 0, 0), 2) + + if not hand_open: + self.recent.clear() + status["reason"] = f"hand not open ({open_fingers} extended fingers)" + return + if depth_m is None: + self.recent.clear() + status["reason"] = "no valid depth at palm" + return + + xyz = self.deproject(palm_px, depth_m) + status["camera_xyz_m"] = [round(v, 4) for v in xyz] + self.recent.append((now, xyz)) + stable = self.is_stable(now, xyz) + status["stable"] = stable + status["stability_samples"] = len(self.recent) + if overlay is not None: + label = f"hand {'STABLE' if stable else 'tracking'} z={depth_m:.2f}m" + cv2.putText(overlay, label, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0) if stable else (0, 165, 255), 2) + if not stable: + return + + point = PointStamped() + point.header.stamp = msg.header.stamp + point.header.frame_id = msg.header.frame_id or "camera_color_optical_frame" + point.point.x, point.point.y, point.point.z = xyz + self.point_pub.publish(point) + 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: + """A finger counts as extended when its tip is farther from the wrist than its PIP joint.""" + wrist = pixels[WRIST] + count = 0 + for tip, pip in zip(FINGER_TIPS, FINGER_PIPS): + tip_dist = math.dist(pixels[tip], wrist) + pip_dist = math.dist(pixels[pip], wrist) + if tip_dist > pip_dist * 1.05: + count += 1 + return count + + def median_depth_m(self, palm_px: tuple[int, int]) -> float | None: + depth = self.latest_depth + if depth is None: + return None + half = max(int(self.args.depth_window_px) // 2, 1) + y0 = max(palm_px[1] - half, 0) + y1 = min(palm_px[1] + half + 1, depth.shape[0]) + x0 = max(palm_px[0] - half, 0) + x1 = min(palm_px[0] + half + 1, depth.shape[1]) + window = depth[y0:y1, x0:x1].astype(np.float64) + scale = 0.001 if self.latest_depth_encoding == "16UC1" else 1.0 + values = window.flatten() * scale + values = values[(values >= self.args.min_depth_m) & (values <= self.args.max_depth_m)] + if values.size < 3: + return None + return float(np.median(values)) + + def deproject(self, pixel: tuple[int, int], depth_m: float) -> tuple[float, float, float]: + k = self.camera_info.k + fx, fy, cx, cy = k[0], k[4], k[2], k[5] + x = (pixel[0] - cx) / fx * depth_m + y = (pixel[1] - cy) / fy * depth_m + return (x, y, depth_m) + + def is_stable(self, now: float, xyz: tuple[float, float, float]) -> bool: + window = [item for item in self.recent if now - item[0] <= self.args.stable_window_seconds] + if len(window) < self.args.stable_min_samples: + return False + if now - window[0][0] < self.args.stable_min_seconds: + return False + return all(math.dist(item[1], xyz) <= self.args.stable_radius_m for item in window) + + def publish_status(self, status: dict[str, object]) -> None: + msg = String() + msg.data = json.dumps(status) + self.status_pub.publish(msg) + + +def parse_bool(value: str) -> bool: + return str(value).strip().lower() in {"1", "true", "yes", "on"} + + +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) + parser.add_argument("--min-extended-fingers", type=int, default=4, + help="open-palm gate: required extended fingers out of 4 (thumb excluded)") + parser.add_argument("--depth-window-px", type=int, default=7) + parser.add_argument("--min-depth-m", type=float, default=0.3) + parser.add_argument("--max-depth-m", type=float, default=1.5) + parser.add_argument("--stable-radius-m", type=float, default=0.05, + help="all samples in the stability window must stay inside this radius") + parser.add_argument("--stable-min-samples", type=int, default=8) + parser.add_argument("--stable-min-seconds", type=float, default=0.8) + parser.add_argument("--stable-window-seconds", type=float, default=1.5) + parser.add_argument("--show-overlay", type=parse_bool, default=True) + args = parser.parse_args() + + rclpy.init() + node = HumanHandDetectionNode(args) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) 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()) diff --git a/tools/run/auto_handover_on_palm.py b/tools/run/auto_handover_on_palm.py new file mode 100755 index 0000000..071c810 --- /dev/null +++ b/tools/run/auto_handover_on_palm.py @@ -0,0 +1,379 @@ +#!/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 json +import os +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" +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, 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회 실행합니다 " + 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] + 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: + last_report = now + remain = deadline - now + print( + f"[Azas] 대기 중... 최근 {args.trigger_window_sec:.1f}초 안정 검출 " + 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() + if rclpy.ok(): + rclpy.shutdown() + 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=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() + + 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}초 안에 안정적인 손바닥이 없어 종료합니다 (로봇 모션 없음). " + "손 검출 화면에서 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", 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] 자동 핸드오버 완료.") + else: + print(f"[FAIL] 핸드오버가 비정상 종료했습니다 (rc={rc}); 위 로그를 확인하세요.") + return rc + + +if __name__ == "__main__": + raise SystemExit(main()) 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/check_measured_dispenser_press_contact.py b/tools/run/check_measured_dispenser_press_contact.py new file mode 100755 index 0000000..4abc12e --- /dev/null +++ b/tools/run/check_measured_dispenser_press_contact.py @@ -0,0 +1,304 @@ +#!/usr/bin/env python3 +"""Move only to measured dispenser press contact check poses. + +This is a hardware check tool, not a recipe runner. It reads measured +press_contact_joints_deg from calibration.yaml, uses Doosan FK to derive the +contact TCP pose, then generates PRE and optional PRESS poses by changing only +Z. It never calls gripper, TCP setup, cup placement, or re-grasp services. +""" + +from __future__ import annotations + +import argparse +import time +from pathlib import Path +from typing import Any + +import rclpy +import yaml +from dsr_msgs2.srv import Fkin, GetCurrentPosx, MoveLine, MoveWait + + +ROOT = Path("/home/ssu/Azas") +CALIBRATION_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +CONFIRM_PHRASE = "ENABLE_CHECK_PRESS_CONTACT" + +DR_BASE = 0 +MOVE_MODE_ABSOLUTE = 0 +SYNC = 0 +BLENDING_SPEED_TYPE_DUPLICATE = 0 +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} + + +def service_name(prefix: str, suffix: str) -> str: + clean = prefix.strip("/") + return f"/{clean}/{suffix}" if clean else f"/{suffix}" + + +def numeric_list(raw: object, label: str, size: int) -> list[float]: + if not isinstance(raw, list) or len(raw) != size: + raise ValueError(f"{label} must be a {size}-item list") + return [float(value) for value in raw] + + +def parse_dispenser_ids(raw: str) -> list[str]: + ids: list[str] = [] + for token in raw.replace(",", " ").split(): + token = token.strip() + if not token: + continue + if token not in {"1", "2", "3", "4"}: + raise ValueError(f"unsupported dispenser id {token!r}; expected 1..4") + ids.append(token) + if not ids: + raise ValueError("no dispenser ids provided") + return ids + + +def load_press_contact_joints(calibration: Path, dispenser_id: str) -> list[float]: + data = yaml.safe_load(calibration.read_text(encoding="utf-8")) or {} + outlet = (data.get("dispenser_outlets") or {}).get(str(dispenser_id)) + if not isinstance(outlet, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing in {calibration}") + status = str(outlet.get("press_contact_status", "")).strip() + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + raise ValueError( + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg is marked " + f"{status!r}; refusing press-contact motion until PRESS{dispenser_id}_CONTACT is re-taught" + ) + return numeric_list( + outlet.get("press_contact_joints_deg"), + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg", + 6, + ) + + +def call_service( + node: Any, + client: Any, + request: Any, + *, + timeout_sec: float, + label: str, +) -> Any: + timeout_sec = max(float(timeout_sec), 0.1) + if not client.wait_for_service(timeout_sec=timeout_sec): + raise RuntimeError(f"{label} service not available: {client.srv_name}") + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) + if not future.done(): + raise RuntimeError(f"{label} response timeout after {timeout_sec:.1f}s") + if future.exception() is not None: + raise RuntimeError(f"{label} exception: {future.exception()}") + response = future.result() + if response is None: + raise RuntimeError(f"{label} returned no response") + return response + + +def xyz_distance_mm(a: list[float], b: list[float]) -> float: + return sum((a[index] - b[index]) ** 2 for index in range(3)) ** 0.5 + + +class PressContactChecker: + def __init__(self, args: argparse.Namespace) -> None: + self.args = args + rclpy.init(args=None) + self.node = rclpy.create_node("azas_check_measured_dispenser_press_contact") + self.fkin = self.node.create_client(Fkin, service_name(args.service_prefix, "motion/fkin")) + self.move_line = self.node.create_client(MoveLine, service_name(args.service_prefix, "motion/move_line")) + self.move_wait = self.node.create_client(MoveWait, service_name(args.service_prefix, "motion/move_wait")) + self.get_posx = self.node.create_client( + GetCurrentPosx, + service_name(args.service_prefix, "aux_control/get_current_posx"), + ) + + def close(self) -> None: + self.node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + 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 = call_service( + self.node, + 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}") + posx = [float(value) for value in response.conv_posx[:6]] + if len(posx) < 6: + raise RuntimeError(f"Fkin returned too few values for {label}: {posx}") + print( + f"[Azas] {label}: contact_fk_posx=[{posx[0]:.1f}, {posx[1]:.1f}, {posx[2]:.1f}, " + f"{posx[3]:.1f}, {posx[4]:.1f}, {posx[5]:.1f}]" + ) + return posx + + def current_posx(self) -> list[float]: + req = GetCurrentPosx.Request() + req.ref = DR_BASE + response = call_service( + self.node, + self.get_posx, + req, + timeout_sec=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 = [float(value) for value in response.task_pos_info[0].data[:6]] + if len(values) < 6: + raise RuntimeError(f"GetCurrentPosx returned too few values: {values}") + return values + + def move_posx(self, target: list[float], label: str, *, velocity: float, acceleration: float) -> None: + print( + f"[Azas] {label}: target_posx=[{target[0]:.1f}, {target[1]:.1f}, {target[2]:.1f}, " + f"{target[3]:.1f}, {target[4]:.1f}, {target[5]:.1f}] vel={velocity:.1f} acc={acceleration:.1f}" + ) + if not self.args.execute: + return + req = MoveLine.Request() + req.pos = [float(value) for value in target] + req.vel = [float(velocity), float(velocity)] + req.acc = [float(acceleration), float(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 = call_service( + self.node, + self.move_line, + req, + timeout_sec=self.args.motion_timeout_sec, + label=f"MoveLine {label}", + ) + if not response.success: + raise RuntimeError(f"MoveLine returned success=false for {label}") + self.wait_motion_done(label) + if self.args.verify_target: + self.wait_for_target(target, label) + + def wait_motion_done(self, label: str) -> None: + response = call_service( + self.node, + self.move_wait, + MoveWait.Request(), + timeout_sec=self.args.motion_timeout_sec, + label=f"MoveWait {label}", + ) + if not bool(getattr(response, "success", True)): + raise RuntimeError(f"MoveWait returned success=false for {label}: {response}") + print(f"[Azas] {label}: MoveWait completed") + + def wait_for_target(self, target: list[float], label: str) -> None: + deadline = time.monotonic() + max(float(self.args.verify_timeout_sec), 0.1) + last_distance = 999999.0 + while time.monotonic() < deadline: + actual = self.current_posx() + last_distance = xyz_distance_mm(actual, target) + print( + f"[Azas] verify {label}: actual_xyz=[{actual[0]:.1f}, {actual[1]:.1f}, {actual[2]:.1f}] " + f"distance={last_distance:.1f}mm tolerance={self.args.target_tolerance_mm:.1f}mm" + ) + if last_distance <= max(float(self.args.target_tolerance_mm), 0.1): + return + time.sleep(max(float(self.args.verify_poll_seconds), 0.05)) + raise RuntimeError(f"target verification timeout for {label}; distance={last_distance:.1f}mm") + + def run_dispenser(self, dispenser_id: str) -> None: + contact_joints = load_press_contact_joints(self.args.calibration, dispenser_id) + print( + f"[Azas] dispenser {dispenser_id}: press_contact_joints_deg=[" + + ", ".join(f"{value:.2f}" for value in contact_joints) + + "]" + ) + contact = self.fkin_posx(contact_joints, f"dispenser {dispenser_id} PRESS_CONTACT") + pre = list(contact) + pre[2] += max(float(self.args.pre_lift_m), 0.0) * 1000.0 + pressed = list(contact) + pressed[2] -= max(float(self.args.press_depth_m), 0.0) * 1000.0 + print( + f"[Azas] dispenser {dispenser_id}: generated PRE=CONTACT+Z{self.args.pre_lift_m * 1000.0:.1f}mm; " + f"optional PRESS=CONTACT-Z{self.args.press_depth_m * 1000.0:.1f}mm" + ) + self.move_posx(pre, f"D{dispenser_id} generated PRESS_PRE", velocity=self.args.travel_velocity, acceleration=self.args.travel_acceleration) + if self.args.stage in {"contact", "press"}: + self.move_posx(contact, f"D{dispenser_id} measured PRESS_CONTACT", velocity=self.args.line_velocity, acceleration=self.args.line_acceleration) + if self.args.stage == "press" and self.args.press_depth_m > 0.0: + self.move_posx(pressed, f"D{dispenser_id} optional Z press", velocity=self.args.line_velocity, acceleration=self.args.line_acceleration) + self.move_posx(contact, f"D{dispenser_id} return to PRESS_CONTACT", velocity=self.args.line_velocity, acceleration=self.args.line_acceleration) + if self.args.return_pre and self.args.stage in {"contact", "press"}: + self.move_posx(pre, f"D{dispenser_id} return to generated PRESS_PRE", velocity=self.args.line_velocity, acceleration=self.args.line_acceleration) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Check measured dispenser press contact positions only.") + parser.add_argument("--dispenser-ids", default="1", help="comma/space separated dispenser ids, e.g. 1 or 1,2,3,4") + parser.add_argument("--calibration", type=Path, default=CALIBRATION_CONFIG) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--stage", choices=["pre", "contact", "press"], default="pre") + parser.add_argument("--pre-lift-m", type=float, default=0.050) + parser.add_argument("--press-depth-m", type=float, default=0.0) + parser.add_argument("--return-pre", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--travel-velocity", type=float, default=12.0) + parser.add_argument("--travel-acceleration", type=float, default=16.0) + parser.add_argument("--line-velocity", type=float, default=6.0) + parser.add_argument("--line-acceleration", type=float, default=10.0) + parser.add_argument("--wait-service-sec", type=float, default=5.0) + parser.add_argument("--motion-timeout-sec", type=float, default=80.0) + parser.add_argument("--verify-target", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--verify-timeout-sec", type=float, default=30.0) + parser.add_argument("--verify-poll-seconds", type=float, default=0.2) + parser.add_argument("--target-tolerance-mm", type=float, default=20.0) + 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() + try: + dispenser_ids = parse_dispenser_ids(args.dispenser_ids) + except ValueError as exc: + print(f"[BLOCKED] {exc}") + return 2 + print("[Azas] Measured dispenser press contact check") + print(f"[Azas] dispenser_ids={','.join(dispenser_ids)} stage={args.stage}") + print("[Azas] no cup/gripper/TCP/re-grasp services are called") + if not args.execute: + print("[BLOCKED] --execute is required for this real press-motion check tool") + return 2 + if args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --confirm must be exactly {CONFIRM_PHRASE}") + return 2 + + checker = PressContactChecker(args) + try: + for dispenser_id in dispenser_ids: + checker.run_dispenser(dispenser_id) + print("[PASS] measured dispenser press contact check completed") + return 0 + except Exception as exc: + print(f"[FAIL] {exc}") + return 1 + finally: + checker.close() + + +if __name__ == "__main__": + raise SystemExit(main()) 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..e45cfba --- /dev/null +++ b/tools/run/check_one_click_cocktail_config.sh @@ -0,0 +1,108 @@ +#!/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_pre_joints_deg", 6, 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..51bc4a9 --- /dev/null +++ b/tools/run/check_one_click_cocktail_result.sh @@ -0,0 +1,94 @@ +#!/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' \ + 'PRESS_PRE measured press pre-contact joints' \ + 'PRESS_CONTACT measured contact joints' \ + 'PRESS_Z_OVERDRIVE from measured contact' \ + 'return to PRESS_CONTACT after Z overdrive' \ + '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/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/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/direct_movel_xyz.py b/tools/run/direct_movel_xyz.py index ae3151e..5ebbae5 100755 --- a/tools/run/direct_movel_xyz.py +++ b/tools/run/direct_movel_xyz.py @@ -168,13 +168,36 @@ def parse_args() -> argparse.Namespace: action="store_true", help="call /motion/ikin before MoveLine and fail closed if the pose is not solvable", ) + parser.add_argument( + "--ikin-timeout-sec", + type=float, + default=20.0, + help="service response timeout for each /motion/ikin precheck attempt", + ) + parser.add_argument( + "--ikin-retries", + type=int, + default=2, + 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) @@ -194,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="", @@ -202,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) @@ -257,31 +297,68 @@ def main() -> int: assert node is not None if args.precheck_ikin: - req = Ikin.Request() - req.pos = pos_mm_deg - req.sol_space = int(args.ikin_sol_space) - req.ref = DR_BASE - response = call_service( - node, - Ikin, - prefixed_service(args.service_prefix, "motion/ikin"), - req, - timeout_sec=max(args.wait_service_sec, 0.1), - label="Ikin", - ) - if not response.success: - print("[FAIL] Ikin returned success=false") + 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): + 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 + if attempt < attempts: + time.sleep(1.0) + 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/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh new file mode 100755 index 0000000..338133e --- /dev/null +++ b/tools/run/dispenser_color_scan_ros.sh @@ -0,0 +1,33 @@ +#!/usr/bin/env bash +# 디스펜서 색상 스캔 (ROS 모드). +# 로봇이 color_scan_pose (joints [0,10,32,0,100,90]°)에 있으면 +# 카메라 화면의 색상 핸들을 직접 검출해 왼쪽→오른쪽을 1→4번으로 저장합니다. +# TF 투영은 visible-handle 검출 실패 시 보조 경로로만 사용됩니다. +set -euo pipefail +ROOT="$(cd "$(dirname "$0")/../.." && pwd)" + +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" + +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:-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/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py new file mode 100755 index 0000000..ac945ff --- /dev/null +++ b/tools/run/handover_cup_to_palm.py @@ -0,0 +1,753 @@ +#!/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 -> 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 + 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" + + +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.""" + + 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 + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + + 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 = 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]]] = [] + 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 + + 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 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 + + 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, + rpy_deg: list[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}", + "--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] + 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})") + + +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}") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + 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("--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=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.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="", + 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() + + +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 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.") + + 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)] + 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] + 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 -> " + 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 + + # --- 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" + ), + 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, + 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") + + # 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] + 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, + 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] 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, + rpy_deg=preserved_rpy) + 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/listen_stt_recipe.py b/tools/run/listen_stt_recipe.py new file mode 100644 index 0000000..628266b --- /dev/null +++ b/tools/run/listen_stt_recipe.py @@ -0,0 +1,94 @@ +#!/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 + + recipe_id = str(data.get("recipe_id", "custom")).strip() + + # 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} + 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/confirmed_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/move_to_measured_dispenser_front_hold.py b/tools/run/move_to_measured_dispenser_front_hold.py index 4712983..522e791 100755 --- a/tools/run/move_to_measured_dispenser_front_hold.py +++ b/tools/run/move_to_measured_dispenser_front_hold.py @@ -192,6 +192,18 @@ def parse_args() -> argparse.Namespace: help="Base-frame Z offset added to the measured link_6 target; use only for derived staging poses.", ) parser.add_argument("--precheck-ikin", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument( + "--ikin-timeout-sec", + type=float, + default=20.0, + help="service response timeout for each /motion/ikin precheck attempt", + ) + parser.add_argument( + "--ikin-retries", + type=int, + default=2, + help="number of /motion/ikin precheck attempts before failing closed", + ) parser.add_argument("--verify-target", action=argparse.BooleanOptionalAction, default=True) parser.add_argument( "--moveit-planning-guard", @@ -248,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", @@ -689,10 +707,26 @@ def main() -> int: f"{args.timeout_sec:.6f}", "--wait-service-sec", f"{args.wait_service_sec:.6f}", + "--ikin-timeout-sec", + f"{args.ikin_timeout_sec:.6f}", + "--ikin-retries", + str(max(int(args.ikin_retries), 1)), "--target-tolerance-mm", 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/open_robot_pipeline_control_panel.sh b/tools/run/open_robot_pipeline_control_panel.sh index 396135e..84709c5 100755 --- a/tools/run/open_robot_pipeline_control_panel.sh +++ b/tools/run/open_robot_pipeline_control_panel.sh @@ -10,6 +10,38 @@ LOG_FILE="$LOG_DIR/robot_pipeline_control_panel.log" 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" +RESTART_SERVER=1 + +case "${1:-}" in + --restart|restart) + RESTART_SERVER=1 + ;; + --reuse|reuse) + RESTART_SERVER=0 + ;; + -h|--help) + cat <&2 + echo "Usage: azas-panel [--restart|--reuse]" >&2 + exit 2 + ;; +esac mkdir -p "$LOG_DIR" @@ -35,6 +67,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" + [[ "$RESTART_SERVER" == "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' @@ -62,11 +138,13 @@ MSG start_panel_server() { cd "$ROOT" - setsid env AZAS_ROOT="$ROOT" 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:-1}" exec python3 tools/run/robot_pipeline_control_server.py ' >> "$LOG_FILE" 2>&1 < /dev/null & echo "$!" > "$PID_FILE" @@ -90,6 +168,16 @@ open_browser() { install_command_symlink ensure_workspace_built +PID="$(server_pid || true)" +if [[ -n "$PID" ]] && server_needs_restart "$PID"; then + if [[ "$RESTART_SERVER" == "1" ]]; then + echo "[Azas] 패널 서버를 새로 초기화합니다." + else + echo "[Azas] 패널 서버 코드 변경 감지: 새 코드로 자동 재시작합니다." + fi + stop_panel_server "$PID" +fi + if ! panel_ready; then echo "[Azas] 패널 서버 시작 중..." start_panel_server @@ -110,4 +198,5 @@ fi open_browser echo "[Azas] 브라우저 열기: $URL" +echo "[Azas] ROS_DOMAIN_ID: $PANEL_ROS_DOMAIN_ID" echo "[Azas] 다음부터는 터미널에서: azas-panel" diff --git a/tools/run/pick_from_cup_holder_side_grip.py b/tools/run/pick_from_cup_holder_side_grip.py new file mode 100755 index 0000000..7ddfaa9 --- /dev/null +++ b/tools/run/pick_from_cup_holder_side_grip.py @@ -0,0 +1,298 @@ +#!/usr/bin/env python3 +"""Pick a closed cup back up from the measured cup holder using side grip. + +This is the reverse of place_side_grip_cup_in_holder.py for the measured +cup_holder.side_grip_place poses in calibration.yaml. It does not invent cup +coordinates: it reuses the measured active-TCP holder place pose. +""" + +from __future__ import annotations + +import argparse +import math +import os +import subprocess +import sys +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import yaml + + +ROOT = Path("/home/ssu/Azas") +DEFAULT_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +DIRECT_MOVEL = ROOT / "tools" / "run" / "direct_movel_xyz.py" +RG2_SET_WIDTH = ROOT / "tools" / "run" / "rg2_set_width_verify.py" +CONFIRM_PHRASE = "ENABLE_CUP_HOLDER_PICK" +DIRECT_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEL" + + +@dataclass(frozen=True) +class TargetPose: + label: str + xyz_m: list[float] + rpy_rad: list[float] + + @property + def rpy_deg(self) -> list[float]: + return [math.degrees(value) for value in self.rpy_rad] + + +def numeric_list(value: Any, label: str, count: int) -> list[float]: + if not isinstance(value, list) or len(value) != count: + raise ValueError(f"{label} must be a {count}-number list") + try: + return [float(item) for item in value] + except (TypeError, ValueError) as exc: + raise ValueError(f"{label} must contain only numbers") from exc + + +def load_target(block: dict[str, Any], name: str) -> TargetPose: + xyz = numeric_list(block.get(f"{name}_pose_xyz_m"), f"{name}_pose_xyz_m", 3) + rpy = numeric_list(block.get(f"{name}_pose_rpy_rad"), f"{name}_pose_rpy_rad", 3) + return TargetPose(name, xyz, rpy) + + +def load_sequence(config_path: Path) -> tuple[TargetPose, TargetPose, TargetPose, float]: + with config_path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + holder = data.get("cup_holder") + if not isinstance(holder, dict): + raise ValueError("cup_holder section is missing in calibration.yaml") + block = holder.get("side_grip_place") + if not isinstance(block, dict): + raise ValueError("cup_holder.side_grip_place section is missing") + + pre_place = load_target(block, "pre_place") + place_final = load_target(block, "place_final") + retreat = load_target(block, "retreat") + approach_lift_m = float(block.get("approach_lift_m", 0.0)) + return pre_place, place_final, retreat, approach_lift_m + + +def offset_target_z(target: TargetPose, offset_m: float) -> TargetPose: + adjusted_xyz = list(target.xyz_m) + adjusted_xyz[2] += float(offset_m) + return TargetPose(target.label, adjusted_xyz, list(target.rpy_rad)) + + +def print_target(target: TargetPose) -> None: + rx, ry, rz = target.rpy_deg + x, y, z = target.xyz_m + print( + f"[Azas] {target.label}: xyz_m=[{x:.6f}, {y:.6f}, {z:.6f}] " + f"doosan_rpy_deg=[{rx:.3f}, {ry:.3f}, {rz:.3f}]" + ) + + +def run_movel( + target: TargetPose, + *, + args: argparse.Namespace, + velocity: float, + acceleration: float, +) -> int: + rx, ry, rz = target.rpy_deg + cmd = [ + sys.executable, + str(DIRECT_MOVEL), + "--service-prefix", + args.service_prefix, + "--x", + f"{target.xyz_m[0]:.6f}", + "--y", + f"{target.xyz_m[1]:.6f}", + "--z", + f"{target.xyz_m[2]:.6f}", + "--rx", + f"{rx:.6f}", + "--ry", + f"{ry:.6f}", + "--rz", + f"{rz:.6f}", + "--velocity", + f"{velocity:.6f}", + "--acceleration", + f"{acceleration:.6f}", + "--timeout-sec", + f"{args.timeout_sec:.6f}", + "--wait-service-sec", + f"{args.wait_service_sec:.6f}", + "--ikin-timeout-sec", + f"{args.ikin_timeout_sec:.6f}", + "--ikin-retries", + str(max(int(args.ikin_retries), 1)), + "--verify-timeout-sec", + f"{args.verify_timeout_sec:.6f}", + "--target-tolerance-mm", + f"{args.target_tolerance_mm:.6f}", + "--x-min", + f"{args.x_min:.6f}", + "--x-max", + f"{args.x_max:.6f}", + "--y-min", + f"{args.y_min:.6f}", + "--y-max", + f"{args.y_max:.6f}", + "--z-min", + f"{args.z_min:.6f}", + "--z-max", + f"{args.z_max:.6f}", + ] + if args.execute: + cmd.extend(["--precheck-ikin", "--verify-target", "--execute", "--confirm", DIRECT_CONFIRM_PHRASE]) + + print(f"[Azas] MoveLine step={target.label}") + sys.stdout.flush() + return subprocess.run(cmd, cwd=str(ROOT), check=False).returncode + + +def run_gripper(args: argparse.Namespace, *, command: str, width_m: float, force_n: float) -> int: + print(f"[Azas] RG2 command={command} width_m={width_m:.3f} force_n={force_n:.1f}") + if not args.execute: + print("[DRY-RUN] --execute not set; gripper command not sent.") + return 0 + + env = os.environ.copy() + env["RG2_SET_WIDTH_SERVICE"] = args.gripper_service + cmd = [ + sys.executable, + str(RG2_SET_WIDTH), + "--service", + args.gripper_service, + "--command", + command, + "--width-m", + f"{width_m:.6f}", + "--force-n", + f"{force_n:.6f}", + "--timeout-sec", + f"{args.gripper_timeout_sec:.3f}", + ] + return subprocess.run(cmd, cwd=str(ROOT), env=env, check=False).returncode + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Open RG2, approach measured cup-holder side-grip pose, grasp, then lift." + ) + parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--approach-velocity", type=float, default=12.0) + parser.add_argument("--approach-acceleration", type=float, default=16.0) + parser.add_argument("--descend-velocity", type=float, default=6.0) + parser.add_argument("--descend-acceleration", type=float, default=10.0) + parser.add_argument("--lift-velocity", type=float, default=12.0) + parser.add_argument("--lift-acceleration", type=float, default=16.0) + parser.add_argument( + "--place-final-z-offset-m", + type=float, + 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) + parser.add_argument("--ikin-timeout-sec", type=float, default=20.0) + parser.add_argument("--ikin-retries", type=int, default=2) + parser.add_argument("--verify-timeout-sec", type=float, default=45.0) + parser.add_argument("--target-tolerance-mm", type=float, default=12.0) + parser.add_argument("--x-min", type=float, default=0.35) + parser.add_argument("--x-max", type=float, default=0.50) + parser.add_argument("--y-min", type=float, default=0.15) + parser.add_argument("--y-max", type=float, default=0.30) + parser.add_argument("--z-min", type=float, default=0.08) + parser.add_argument("--z-max", type=float, default=0.28) + 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=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") + parser.add_argument( + "--confirm", + default="", + help=f"must equal {CONFIRM_PHRASE} when --execute is used", + ) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + if not args.config.is_file(): + print(f"[FAIL] calibration config not found: {args.config}") + return 2 + if args.execute and args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --confirm must be exactly {CONFIRM_PHRASE}") + return 2 + + try: + pre_place, place_final, retreat, approach_lift_m = load_sequence(args.config) + if abs(args.place_final_z_offset_m) > 1e-9: + place_final = offset_target_z(place_final, args.place_final_z_offset_m) + except (OSError, ValueError, yaml.YAMLError) as exc: + print(f"[FAIL] {exc}") + return 2 + + print("[Azas] Cup holder side-grip pick sequence") + print(f"[Azas] config={args.config}") + print(f"[Azas] service_prefix={args.service_prefix}") + print("[Azas] source=cup_holder.side_grip_place measured poses; no operator/LLM-generated cup coordinates") + print(f"[Azas] approach_lift_m={approach_lift_m:.3f}") + print(f"[Azas] place_final_z_offset_m={args.place_final_z_offset_m:.4f}") + print_target(retreat) + print_target(place_final) + print_target(pre_place) + if not args.execute: + print("[DRY-RUN] --execute not set; no robot or gripper command will be sent.") + + rc = run_gripper( + args, + command="open", + width_m=args.gripper_open_width_m, + force_n=args.gripper_force_n, + ) + if rc != 0: + print("[FAIL] gripper open failed; holder approach skipped.") + return rc + + for target, velocity, acceleration in [ + (retreat, args.approach_velocity, args.approach_acceleration), + (place_final, args.descend_velocity, args.descend_acceleration), + ]: + rc = run_movel(target, args=args, velocity=velocity, acceleration=acceleration) + if rc != 0: + print(f"[FAIL] {target.label} MoveLine failed; aborting holder pick.") + return rc + + rc = run_gripper( + args, + command="set_width", + width_m=args.gripper_grasp_width_m, + force_n=args.gripper_force_n, + ) + if rc != 0: + print("[FAIL] gripper grasp failed; lift skipped to avoid dragging an unsecured cup.") + return rc + if args.post_grasp_settle_sec > 0: + print(f"[Azas] post-grasp settle before lift: {args.post_grasp_settle_sec:.2f}s") + import time + time.sleep(args.post_grasp_settle_sec) + + rc = run_movel( + pre_place, + args=args, + velocity=args.lift_velocity, + acceleration=args.lift_acceleration, + ) + if rc != 0: + print("[FAIL] post-grasp holder lift failed.") + return rc + + print("[PASS] cup holder side-grip pick sequence completed") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/pick_from_measured_dispenser_front_hold.py b/tools/run/pick_from_measured_dispenser_front_hold.py index c68fcbc..b59678a 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", @@ -392,13 +404,13 @@ 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) 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) @@ -449,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( @@ -465,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/place_side_grip_cup_in_holder.py b/tools/run/place_side_grip_cup_in_holder.py index 668f55b..ce101c5 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 @@ -86,6 +87,24 @@ def offset_target_z(target: TargetPose, offset_m: float) -> TargetPose: return TargetPose(target.label, adjusted_xyz, list(target.rpy_rad)) +def offset_target_y(target: TargetPose, offset_m: float) -> TargetPose: + adjusted_xyz = list(target.xyz_m) + adjusted_xyz[1] += float(offset_m) + 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)) + 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 @@ -95,6 +114,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, *, @@ -173,21 +293,45 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) parser.add_argument("--service-prefix", default="dsr01") - parser.add_argument("--approach-velocity", type=float, default=15.0) + parser.add_argument("--approach-velocity", type=float, default=80.0) parser.add_argument("--approach-acceleration", type=float, default=20.0) - parser.add_argument("--place-velocity", type=float, default=6.0) + parser.add_argument("--place-velocity", type=float, default=80.0) parser.add_argument("--place-acceleration", type=float, default=10.0) - parser.add_argument("--retreat-velocity", type=float, default=12.0) + parser.add_argument("--retreat-velocity", type=float, default=80.0) parser.add_argument("--retreat-acceleration", type=float, default=16.0) parser.add_argument( "--place-final-z-offset-m", type=float, - default=0.0, + 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." ), ) + 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, + default=-0.010, + help=( + "Measured adjustment added only to place_final Y. Default -0.010m shifts " + "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) @@ -202,6 +346,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", @@ -222,8 +385,16 @@ 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: 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 @@ -232,37 +403,53 @@ 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}") print_target(pre_place) print_target(place_final) print_target(retreat) 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/preview_measured_dispenser_recipe_rviz.py b/tools/run/preview_measured_dispenser_recipe_rviz.py new file mode 100755 index 0000000..9c5814f --- /dev/null +++ b/tools/run/preview_measured_dispenser_recipe_rviz.py @@ -0,0 +1,574 @@ +#!/usr/bin/env python3 +"""RViz-only preview of measured cocktail dispenser recipe joints. + +This script reads joint teaching values from calibration.yaml and publishes only +sensor_msgs/JointState so RViz can animate the M0609 RobotModel in sequence. +It never calls real robot services, MoveJoint, MoveLine, gripper services, or +MoveIt execution. SAFE_LIFT and PRESS_Z_OVERDRIVE_40MM are logged as real-code +TCP linear motions, but this preview holds the measured joint pose for safety. +""" + +from __future__ import annotations + +import argparse +import math +from dataclasses import dataclass +from pathlib import Path + +import rclpy +import yaml +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from sensor_msgs.msg import JointState +from visualization_msgs.msg import Marker, MarkerArray + +DEFAULT_CALIBRATION = Path("/home/ssu/Azas/src/azas_bringup/config/calibration.yaml") +DEFAULT_JOINT_NAMES = "joint_1,joint_2,joint_3,joint_4,joint_5,joint_6" +ALLOWED_DISPENSER_IDS = {"1", "2", "3", "4"} +JOINT_COUNT = 6 +DEFAULT_JOINT_VELOCITY_DEG_S = 40.0 +DEFAULT_PRESS_DEPTH_M = 0.040 + +STAGES = ( + "DISP_PRE", + "DISP_PLACE", + "RELEASE", + "SAFE_LIFT", + "PRESS_PRE", + "PRESS_CONTACT", + "PRESS_Z_OVERDRIVE", + "PRESS_CONTACT_RETURN", + "PRESS_PRE_RETURN", + "DISP_PRE_REGRASP", + "DISP_PLACE_REGRASP", +) + + +def log(message: str) -> None: + print(message, flush=True) + + +@dataclass(frozen=True) +class Waypoint: + order: int + dispenser_id: str + stage: str + joints_deg: list[float] + note: str = "" + taught_joints_deg: list[float] | None = None + + @property + def source_joints_deg(self) -> list[float]: + return self.taught_joints_deg if self.taught_joints_deg is not None else self.joints_deg + + +def parse_dispenser_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}") + 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 ALLOWED_DISPENSER_IDS] + if invalid: + allowed = ",".join(sorted(ALLOWED_DISPENSER_IDS)) + raise ValueError(f"unsupported dispenser id(s): {', '.join(invalid)}; allowed: {allowed}") + return values + + +def parse_joint_names(raw: str) -> list[str]: + names = [item.strip() for item in raw.split(",") if item.strip()] + if len(names) != JOINT_COUNT: + raise ValueError(f"--joint-names must provide exactly {JOINT_COUNT} names") + return names + + +def parse_joint_index_set(raw: str) -> set[int]: + indexes: set[int] = set() + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: + continue + if item.startswith("joint_"): + item = item[6:] + elif item.startswith("j"): + item = item[1:] + try: + index = int(item) + except ValueError as exc: + raise ValueError(f"--press-lock-contact-joints contains a non-joint value: {part!r}") from exc + if not 1 <= index <= JOINT_COUNT: + raise ValueError(f"--press-lock-contact-joints joint index must be 1..{JOINT_COUNT}, got {part!r}") + indexes.add(index - 1) + return indexes + + +def read_yaml(path: Path) -> dict: + data = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + if not isinstance(data, dict): + raise ValueError(f"YAML root must be a map: {path}") + return data + + +def numeric_joints(raw: object, label: str) -> list[float]: + if not isinstance(raw, list) or len(raw) != JOINT_COUNT: + raise ValueError(f"{label} must be a {JOINT_COUNT}-item list") + try: + return [float(value) for value in raw] + except (TypeError, ValueError) as exc: + raise ValueError(f"{label} must contain numeric degree values") from exc + + +def load_outlet_joints(calibration: dict, dispenser_id: str) -> dict[str, list[float]]: + outlets = calibration.get("dispenser_outlets") + if not isinstance(outlets, dict): + raise ValueError("calibration.yaml is missing dispenser_outlets") + + outlet = outlets.get(dispenser_id) + if not isinstance(outlet, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing") + + required_fields = ( + "cup_pre_place_joints_deg", + "cup_place_joints_deg", + "press_pre_joints_deg", + "press_contact_joints_deg", + ) + joints: dict[str, list[float]] = {} + for field in required_fields: + joints[field] = numeric_joints( + outlet.get(field), + f"dispenser_outlets.{dispenser_id}.{field}", + ) + return joints + + +def lock_contact_joints_to_pre( + press_contact: list[float], + press_pre: list[float], + locked_joint_indexes: set[int], +) -> tuple[list[float], str]: + if not locked_joint_indexes: + return list(press_contact), "" + locked = list(press_contact) + labels: list[str] = [] + for index in sorted(locked_joint_indexes): + original = locked[index] + locked[index] = press_pre[index] + labels.append(f"joint_{index + 1} {original:.2f}->{locked[index]:.2f}") + return locked, "press-lock-contact-joints: " + ", ".join(labels) + + +def build_waypoints( + calibration: dict, + dispenser_ids: list[str], + locked_press_contact_joints: set[int], + press_overdrive_mm: float, +) -> list[Waypoint]: + waypoints: list[Waypoint] = [] + for order, dispenser_id in enumerate(dispenser_ids, start=1): + joints = load_outlet_joints(calibration, dispenser_id) + cup_pre = joints["cup_pre_place_joints_deg"] + cup_place = joints["cup_place_joints_deg"] + press_pre = joints["press_pre_joints_deg"] + press_contact, press_contact_note = lock_contact_joints_to_pre( + joints["press_contact_joints_deg"], + press_pre, + locked_press_contact_joints, + ) + + waypoints.extend( + [ + Waypoint(order, dispenser_id, "DISP_PRE", cup_pre), + Waypoint(order, dispenser_id, "DISP_PLACE", cup_place), + Waypoint(order, dispenser_id, "RELEASE", cup_place, "preview holds DISP_PLACE"), + Waypoint( + order, + dispenser_id, + "SAFE_LIFT", + cup_place, + "real code has safe_lift_current TCP MoveLine; preview holds DISP_PLACE", + ), + Waypoint(order, dispenser_id, "PRESS_PRE", press_pre), + Waypoint(order, dispenser_id, "PRESS_CONTACT", press_contact, press_contact_note), + # TODO: replace this hold with a preview-only FK/IK joint state for + # the 40 mm TCP-Z overdrive once a non-hardware kinematics path is + # available. + Waypoint( + order, + dispenser_id, + f"PRESS_Z_OVERDRIVE_{press_overdrive_mm:.0f}MM", + press_contact, + ( + f"real code uses TCP Z MoveLine {press_overdrive_mm:.1f}mm; " + "preview holds PRESS_CONTACT" + ), + ), + Waypoint(order, dispenser_id, "PRESS_CONTACT_RETURN", press_contact), + Waypoint(order, dispenser_id, "PRESS_PRE_RETURN", press_pre), + Waypoint(order, dispenser_id, "DISP_PRE_REGRASP", cup_pre), + Waypoint(order, dispenser_id, "DISP_PLACE_REGRASP", cup_place), + ] + ) + return waypoints + + +def nearest_equivalent_deg(target: float, current: float) -> float: + candidates = [target + 360.0 * offset for offset in range(-2, 3)] + bounded = [candidate for candidate in candidates if abs(candidate) <= 360.0] + if bounded: + candidates = bounded + return min(candidates, key=lambda candidate: abs(candidate - current)) + + +def unwrap_waypoints(waypoints: list[Waypoint]) -> list[Waypoint]: + if not waypoints: + return [] + + unwrapped: list[Waypoint] = [] + previous = waypoints[0].joints_deg + unwrapped.append(waypoints[0]) + for waypoint in waypoints[1:]: + adjusted = [ + nearest_equivalent_deg(target, current) + for target, current in zip(waypoint.joints_deg, previous) + ] + previous = adjusted + unwrapped.append( + Waypoint( + order=waypoint.order, + dispenser_id=waypoint.dispenser_id, + stage=waypoint.stage, + joints_deg=adjusted, + note=waypoint.note, + taught_joints_deg=waypoint.joints_deg, + ) + ) + return unwrapped + + +def interpolate_deg(start: list[float], end: list[float], ratio: float) -> list[float]: + ratio = max(0.0, min(1.0, ratio)) + return [a + (b - a) * ratio for a, b in zip(start, end)] + + +def format_joints(values: list[float]) -> str: + return "[" + ", ".join(f"{value:.2f}" for value in values) + "]" + + +class JointStatePreviewNode(Node): + def __init__( + self, + waypoints: list[Waypoint], + joint_names: list[str], + joint_topic: str, + marker_topic: str, + rate_hz: float, + segment_seconds: float, + joint_velocity_deg_s: float, + hold_seconds: float, + loop: bool, + ) -> None: + super().__init__("preview_measured_dispenser_recipe_rviz") + self._publisher = self.create_publisher(JointState, joint_topic, 10) + self._marker_publisher = self.create_publisher(MarkerArray, marker_topic, 10) if marker_topic else None + self._waypoints = waypoints + self._joint_names = joint_names + self._marker_topic = marker_topic + self._loop = loop + self._rate_hz = rate_hz + self._segment_seconds = segment_seconds + self._joint_velocity_deg_s = joint_velocity_deg_s + self._segment_frames = self._segment_frame_count(0, loop_to_start=False) + self._hold_frames = max(1, int(round(hold_seconds * rate_hz))) + self._index = 0 + self._phase = "hold" + self._phase_frame = 0 + self._looping_to_start = False + self._announced_current = False + self._done = False + self._timer = self.create_timer(1.0 / rate_hz, self._tick) + + log("[Azas] RViz-only measured dispenser recipe preview") + log("[Azas] publishing JointState only; service_calls=none") + log("[Azas] this process does not open RViz; use tools/run/show_measured_recipe_joint_preview_rviz.sh to open RViz too") + log(f"[Azas] joint_topic={joint_topic}") + if marker_topic: + log(f"[Azas] marker_topic={marker_topic}") + log(f"[Azas] joint_names={','.join(joint_names)}") + if joint_velocity_deg_s > 0.0: + log(f"[Azas] preview_joint_velocity_deg_s={joint_velocity_deg_s:.2f}") + else: + log(f"[Azas] preview_fixed_segment_seconds={segment_seconds:.2f}") + log( + "[Azas] sequence=" + + " -> ".join(STAGES) + ) + + @property + def done(self) -> bool: + return self._done + + def _tick(self) -> None: + if self._phase == "hold": + waypoint = self._waypoints[self._index] + if not self._announced_current: + self._announce_stage(waypoint) + self._announced_current = True + self._publish(waypoint.joints_deg) + self._phase_frame += 1 + if self._phase_frame >= self._hold_frames: + self._phase_frame = 0 + if self._index >= len(self._waypoints) - 1: + if self._loop: + self._phase = "segment" + self._looping_to_start = True + self._segment_frames = self._segment_frame_count(self._index, loop_to_start=True) + else: + log("[Azas] preview_complete") + self._done = True + self._timer.cancel() + else: + self._phase = "segment" + self._segment_frames = self._segment_frame_count(self._index, loop_to_start=False) + return + + target_index = 0 if self._looping_to_start else self._index + 1 + start = self._waypoints[self._index].joints_deg + end = self._waypoints[target_index].joints_deg + ratio = (self._phase_frame + 1) / self._segment_frames + self._publish(interpolate_deg(start, end, ratio)) + self._phase_frame += 1 + if self._phase_frame >= self._segment_frames: + self._index = target_index + self._phase = "hold" + self._phase_frame = 0 + self._looping_to_start = False + self._announced_current = False + + def _segment_frame_count(self, start_index: int, *, loop_to_start: bool) -> int: + if self._joint_velocity_deg_s <= 0.0: + seconds = self._segment_seconds + else: + target_index = 0 if loop_to_start else min(start_index + 1, len(self._waypoints) - 1) + start = self._waypoints[start_index].joints_deg + end = self._waypoints[target_index].joints_deg + max_delta_deg = max(abs(a - b) for a, b in zip(start, end)) + seconds = max_delta_deg / self._joint_velocity_deg_s if max_delta_deg > 0.0 else 0.0 + seconds = max(seconds, 1.0 / self._rate_hz) + return max(1, int(math.ceil(seconds * self._rate_hz))) + + def _announce_stage(self, waypoint: Waypoint) -> None: + source = waypoint.source_joints_deg + published = waypoint.joints_deg + suffix = "" + if any(abs(a - b) > 1e-6 for a, b in zip(source, published)): + suffix = f" publish_joint_deg={format_joints(published)}" + log( + f"[Azas] dispenser={waypoint.dispenser_id} " + f"cycle={waypoint.order} stage={waypoint.stage} " + f"joint_deg={format_joints(source)}{suffix}" + ) + if waypoint.note: + log(f"[Azas] {waypoint.stage} note={waypoint.note}") + self._publish_stage_marker(waypoint) + + def _publish(self, joints_deg: list[float]) -> None: + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = self._joint_names + msg.position = [math.radians(value) for value in joints_deg] + self._publisher.publish(msg) + + def _publish_stage_marker(self, waypoint: Waypoint) -> None: + if self._marker_publisher is None: + return + marker = Marker() + marker.header.frame_id = "base_link" + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = "measured_joint_preview_stage" + marker.id = 1 + marker.type = Marker.TEXT_VIEW_FACING + marker.action = Marker.ADD + marker.pose.position.x = 0.42 + marker.pose.position.y = -0.42 + marker.pose.position.z = 0.72 + marker.pose.orientation.w = 1.0 + marker.scale.z = 0.055 + marker.color.r = 0.92 + marker.color.g = 0.96 + marker.color.b = 1.0 + marker.color.a = 1.0 + marker.text = f"D{waypoint.dispenser_id} cycle {waypoint.order}\\n{waypoint.stage}" + if waypoint.note: + marker.text += f"\\n{waypoint.note}" + marker_array = MarkerArray() + marker_array.markers.append(marker) + self._marker_publisher.publish(marker_array) + + +def positive_float(value: str) -> float: + parsed = float(value) + if parsed <= 0.0: + raise argparse.ArgumentTypeError("must be > 0") + return parsed + + +def nonnegative_float(value: str) -> float: + parsed = float(value) + if parsed < 0.0: + raise argparse.ArgumentTypeError("must be >= 0") + return parsed + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Publish measured dispenser recipe joints as JointState for RViz only. " + "No robot, gripper, MoveIt execution, MoveJoint, or MoveLine service is called." + ) + ) + parser.add_argument( + "--dispenser-ids", + default="1", + help="Physical dispenser sequence. Supports values like 1,2,3,4 or 1x2,3.", + ) + parser.add_argument( + "--calibration", + type=Path, + default=DEFAULT_CALIBRATION, + help=f"Path to calibration.yaml (default: {DEFAULT_CALIBRATION})", + ) + parser.add_argument( + "--rate-hz", + "--publish-rate-hz", + dest="rate_hz", + type=positive_float, + default=30.0, + ) + parser.add_argument("--segment-seconds", type=positive_float, default=2.0) + parser.add_argument( + "--joint-velocity-deg-s", + type=nonnegative_float, + default=DEFAULT_JOINT_VELOCITY_DEG_S, + help=( + "Preview interpolation speed in deg/s. Default 40. Set 0 to use " + "--segment-seconds as a fixed duration for every waypoint transition." + ), + ) + parser.add_argument("--hold-seconds", type=nonnegative_float, default=1.0) + parser.add_argument( + "--press-lock-contact-joints", + default="", + help=( + "Compatibility with real recipe commands: comma-separated joint numbers " + "copied from PRESS_PRE into PRESS_CONTACT for preview, e.g. 6." + ), + ) + parser.add_argument("--press-depth-m", type=nonnegative_float, default=DEFAULT_PRESS_DEPTH_M) + parser.add_argument("--press-extra-depth-m", type=nonnegative_float, default=0.0) + parser.add_argument( + "--press-use-recorded-pre-joints", + action=argparse.BooleanOptionalAction, + default=True, + help="Compatibility no-op: preview always uses calibration.yaml press_pre_joints_deg.", + ) + parser.add_argument( + "--press-reset-before-press", + action=argparse.BooleanOptionalAction, + default=False, + help="Compatibility no-op: preview never inserts the reset joint pose.", + ) + parser.add_argument( + "--safe-lift-joint-fallback", + action=argparse.BooleanOptionalAction, + default=True, + help="Compatibility no-op: preview logs SAFE_LIFT but does not call IK/MoveJoint.", + ) + parser.add_argument( + "--unwrap-joints", + action=argparse.BooleanOptionalAction, + default=True, + help="Use nearest equivalent joint angles for smooth RViz interpolation across +/-180/360 boundaries.", + ) + loop_group = parser.add_mutually_exclusive_group() + loop_group.add_argument("--loop", dest="loop", action="store_true", help="Repeat the preview until interrupted.") + loop_group.add_argument("--no-loop", dest="loop", action="store_false", help="Play once and exit.") + parser.set_defaults(loop=False) + parser.add_argument("--joint-topic", "--joint-state-topic", dest="joint_topic", default="/joint_states") + parser.add_argument("--marker-topic", default="/azas/measured_joint_preview/markers") + parser.add_argument("--joint-names", default=DEFAULT_JOINT_NAMES) + args, unknown = parser.parse_known_args() + args.ignored_real_motion_args = unknown + return args + + +def main() -> int: + args = parse_args() + try: + dispenser_ids = parse_dispenser_ids(args.dispenser_ids) + joint_names = parse_joint_names(args.joint_names) + locked_press_contact_joints = parse_joint_index_set(args.press_lock_contact_joints) + press_overdrive_mm = (args.press_depth_m + args.press_extra_depth_m) * 1000.0 + calibration = read_yaml(args.calibration) + waypoints = build_waypoints( + calibration, + dispenser_ids, + locked_press_contact_joints, + press_overdrive_mm, + ) + if args.unwrap_joints: + waypoints = unwrap_waypoints(waypoints) + except (OSError, ValueError) as exc: + log(f"[Azas] preview setup failed: {exc}") + return 2 + + if not waypoints: + log("[Azas] preview setup failed: no waypoints") + return 2 + + rclpy.init() + node = JointStatePreviewNode( + waypoints=waypoints, + joint_names=joint_names, + joint_topic=args.joint_topic, + marker_topic=args.marker_topic, + rate_hz=args.rate_hz, + segment_seconds=args.segment_seconds, + joint_velocity_deg_s=args.joint_velocity_deg_s, + hold_seconds=args.hold_seconds, + loop=args.loop, + ) + if args.ignored_real_motion_args: + log("[Azas] ignored_real_motion_args=" + " ".join(args.ignored_real_motion_args)) + try: + while rclpy.ok() and not node.done: + rclpy.spin_once(node, timeout_sec=0.1) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + 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/publish_color_recipe_sequence_rviz_preview.py b/tools/run/publish_color_recipe_sequence_rviz_preview.py new file mode 100644 index 0000000..5553025 --- /dev/null +++ b/tools/run/publish_color_recipe_sequence_rviz_preview.py @@ -0,0 +1,394 @@ +#!/usr/bin/env python3 +"""Publish an RViz-only mirror of run_color_recipe_sequence.py. + +This script reads the same color map and recipe inputs used by +run_color_recipe_sequence.py, derives the same physical dispenser order, then +publishes a Path and MarkerArray. It never calls Doosan motion, gripper, camera, +or execution services. +""" + +from __future__ import annotations + +import argparse +import json +import sys +from dataclasses import dataclass +from pathlib import Path +from typing import Iterable + +import rclpy +import yaml +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from nav_msgs.msg import Path as RosPath +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile +from visualization_msgs.msg import Marker, MarkerArray + +ROOT = Path(__file__).resolve().parents[2] +RUN_DIR = ROOT / "tools" / "run" +sys.path.insert(0, str(RUN_DIR)) + +from run_color_recipe_sequence import ( # noqa: E402 + RECIPE_PATH, + color_to_dispenser_id, + load_color_map, + parse_colors_arg, + parse_direct_dispenser_sequence, +) + +DEFAULT_DISPENSER_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "measured_dispenser_collision.yaml" +DEFAULT_CALIBRATION = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" + + +XYZ = tuple[float, float, float] +RGBA = tuple[float, float, float, float] + + +@dataclass(frozen=True) +class Step: + label: str + xyz: XYZ + color: RGBA + scale: float = 0.028 + + +def point(xyz: XYZ) -> Point: + return Point(x=float(xyz[0]), y=float(xyz[1]), z=float(xyz[2])) + + +def pose(xyz: XYZ) -> Pose: + msg = Pose() + msg.position = point(xyz) + msg.orientation = Quaternion(w=1.0) + return msg + + +def add(a: XYZ, b: XYZ) -> XYZ: + return (a[0] + b[0], a[1] + b[1], a[2] + b[2]) + + +def read_yaml(path: Path) -> dict: + data = yaml.safe_load(path.read_text(encoding="utf-8")) + if not isinstance(data, dict): + raise ValueError(f"YAML is not a map: {path}") + return data + + +def read_front_holds(path: Path) -> dict[str, XYZ]: + data = read_yaml(path) + poses = data.get("front_hold_poses") or {} + if not isinstance(poses, dict): + raise ValueError(f"front_hold_poses is missing in {path}") + result: dict[str, XYZ] = {} + for dispenser_id in ("1", "2", "3", "4"): + block = poses.get(f"dispenser_{dispenser_id}") or {} + xyz = block.get("position_xyz_m") + if not isinstance(xyz, list) or len(xyz) != 3: + raise ValueError(f"front_hold_poses.dispenser_{dispenser_id}.position_xyz_m is invalid") + result[dispenser_id] = (float(xyz[0]), float(xyz[1]), float(xyz[2])) + return result + + +def read_press_poses(path: Path) -> dict[str, XYZ]: + data = read_yaml(path) + outlets = data.get("dispenser_outlets") or {} + if not isinstance(outlets, dict): + raise ValueError(f"dispenser_outlets is missing in {path}") + result: dict[str, XYZ] = {} + for dispenser_id in ("1", "2", "3", "4"): + block = outlets.get(dispenser_id) or {} + xyz = block.get("press_pose_xyz_m") + if not isinstance(xyz, list) or len(xyz) != 3: + raise ValueError(f"dispenser_outlets.{dispenser_id}.press_pose_xyz_m is invalid") + result[dispenser_id] = (float(xyz[0]), float(xyz[1]), float(xyz[2])) + return result + + +def group_consecutive(ids: Iterable[str]) -> list[tuple[str, int]]: + grouped: list[tuple[str, int]] = [] + for dispenser_id in ids: + if grouped and grouped[-1][0] == dispenser_id: + grouped[-1] = (dispenser_id, grouped[-1][1] + 1) + else: + grouped.append((dispenser_id, 1)) + return grouped + + +def derive_sequence(args: argparse.Namespace) -> tuple[list[str], dict[str, str], list[tuple[str, int]]]: + if args.dispenser_ids.strip(): + sequence = parse_direct_dispenser_sequence(args.dispenser_ids) + return sequence, {}, [] + + color_map = load_color_map(override_json=args.color_map_json) + if not color_map: + if not args.allow_missing_color_map_fallback: + raise RuntimeError("outputs/dispenser_color_map.json is missing/invalid") + return ["1", "2", "3", "4"], {}, [] + + if args.colors.strip(): + color_pumps = parse_colors_arg(args.colors) + else: + if not RECIPE_PATH.exists(): + raise RuntimeError(f"recipe is missing: {RECIPE_PATH}") + recipe = json.loads(RECIPE_PATH.read_text(encoding="utf-8")) + colors = recipe.get("colors", []) + pumps = recipe.get("pumps", {}) + color_pumps = [(str(color).lower(), int(pumps.get(color, 1))) for color in colors] + + sequence: list[str] = [] + for color, count in color_pumps: + dispenser_id = color_to_dispenser_id(color, color_map) + if dispenser_id is None: + raise RuntimeError(f"color {color!r} is not present in color map {color_map}") + sequence.extend([dispenser_id] * count) + if not sequence: + raise RuntimeError("derived dispenser sequence is empty") + return sequence, color_map, color_pumps + + +def build_steps(args: argparse.Namespace, sequence: list[str]) -> list[Step]: + front_holds = read_front_holds(args.dispenser_config) + press_poses = read_press_poses(args.calibration) + + steps: list[Step] = [] + grouped = group_consecutive(sequence) + for group_index, (dispenser_id, press_count) in enumerate(grouped, start=1): + hold = front_holds[dispenser_id] + press = press_poses[dispenser_id] + release = add( + hold, + ( + args.move_release_offset_x_m, + args.move_release_offset_y_m, + args.move_release_offset_z_m, + ), + ) + prehold = add(hold, (args.move_prehold_offset_x_m, args.move_prehold_offset_y_m, args.move_prehold_offset_z_m)) + above = add(hold, (0.0, 0.0, args.move_prehold_offset_z_m)) + press_retreat = add(release, (args.press_pre_lift_retreat_x_m, args.press_pre_lift_retreat_y_m, 0.0)) + empty_lift = ( + press_retreat[0], + press_retreat[1], + max(args.press_min_transit_z_m, release[2] + args.press_transit_height_m), + ) + press_ready = (press[0], press[1], press[2] + args.press_pre_lift_m) + press_down = (press[0], press[1], press[2] - args.press_depth_m) + post_press_lift = ( + press[0], + press[1], + max(args.regrasp_min_transit_z_m, press[2] + args.press_pre_lift_m), + ) + post_press_retreat = add( + post_press_lift, + (args.regrasp_retreat_x_m, args.regrasp_retreat_y_m, 0.0), + ) + rear_high = ( + release[0] + args.regrasp_rear_entry_offset_x_m, + release[1] + args.regrasp_rear_entry_offset_y_m, + min(max(args.regrasp_min_transit_z_m, release[2] + args.regrasp_approach_offset_z_m), args.regrasp_max_transit_z_m), + ) + rear_low = ( + release[0] + args.regrasp_rear_entry_offset_x_m, + release[1] + args.regrasp_rear_entry_offset_y_m, + release[2], + ) + regrasp_lift = (release[0], release[1], release[2] + args.pick_lift_m) + + prefix = f"G{group_index} D{dispenser_id}x{press_count}" + steps.extend( + [ + Step(f"{prefix} prehold", prehold, (0.1, 0.55, 1.0, 1.0)), + Step(f"{prefix} above_front_hold", above, (0.1, 0.75, 1.0, 1.0)), + Step(f"{prefix} RELEASE front_hold exact", release, (0.0, 1.0, 0.25, 1.0), 0.04), + Step(f"{prefix} X- retreat before press lift", press_retreat, (1.0, 0.9, 0.1, 1.0)), + Step(f"{prefix} safe lift", empty_lift, (1.0, 0.9, 0.1, 1.0)), + Step(f"{prefix} press ready", press_ready, (1.0, 0.4, 0.1, 1.0)), + Step(f"{prefix} press down", press_down, (1.0, 0.0, 0.0, 1.0), 0.035), + Step(f"{prefix} lift after press", post_press_lift, (1.0, 0.4, 0.1, 1.0)), + Step(f"{prefix} X- retreat then RG2 open", post_press_retreat, (1.0, 0.8, 0.0, 1.0), 0.04), + Step(f"{prefix} rear high", rear_high, (0.7, 0.2, 1.0, 1.0)), + Step(f"{prefix} rear low", rear_low, (0.7, 0.2, 1.0, 1.0)), + Step(f"{prefix} forward regrasp front_hold", release, (0.0, 1.0, 0.9, 1.0), 0.04), + Step(f"{prefix} regrasp lift", regrasp_lift, (0.0, 0.8, 1.0, 1.0)), + ] + ) + return steps + + +class ColorRecipePreviewPublisher(Node): + def __init__(self, args: argparse.Namespace, steps: list[Step], sequence: list[str], color_map: dict[str, str], color_pumps: list[tuple[str, int]]) -> None: + super().__init__("azas_color_recipe_sequence_rviz_preview") + self.args = args + self.steps = steps + self.sequence = sequence + self.color_map = color_map + self.color_pumps = color_pumps + qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self.path_pub = self.create_publisher(RosPath, "/azas/dispenser_sequence/plan", qos) + self.marker_pub = self.create_publisher(MarkerArray, "/azas/dispenser_sequence/markers", qos) + self.create_timer(1.0 / max(args.publish_rate_hz, 0.2), self.publish_preview) + self.get_logger().info( + "RViz mirror of run_color_recipe_sequence.py ready; no robot/gripper/camera commands are sent" + ) + self.get_logger().info(f"sequence={','.join(sequence)} grouped={group_consecutive(sequence)}") + + def publish_preview(self) -> None: + stamp = self.get_clock().now().to_msg() + path = RosPath() + path.header.frame_id = self.args.frame_id + path.header.stamp = stamp + for step in self.steps: + msg = PoseStamped() + msg.header = path.header + msg.pose = pose(step.xyz) + path.poses.append(msg) + self.path_pub.publish(path) + self.marker_pub.publish(self.make_markers(stamp)) + + def make_markers(self, stamp) -> MarkerArray: + markers: list[Marker] = [] + clear = Marker() + clear.header.frame_id = self.args.frame_id + clear.header.stamp = stamp + clear.action = Marker.DELETEALL + markers.append(clear) + + line = Marker() + line.header.frame_id = self.args.frame_id + line.header.stamp = stamp + line.ns = "actual_color_recipe_path" + line.id = 1 + line.type = Marker.LINE_STRIP + line.action = Marker.ADD + line.pose.orientation.w = 1.0 + line.scale.x = 0.018 + line.color.r = 1.0 + line.color.g = 0.9 + line.color.b = 0.0 + line.color.a = 1.0 + line.points = [point(step.xyz) for step in self.steps] + markers.append(line) + + for idx, step in enumerate(self.steps, start=10): + markers.append(self.sphere_marker(idx, step, stamp)) + markers.append(self.text_marker(idx + 10000, step, stamp)) + markers.append(self.command_text_marker(90000, stamp)) + return MarkerArray(markers=markers) + + def sphere_marker(self, marker_id: int, step: Step, stamp) -> Marker: + marker = Marker() + marker.header.frame_id = self.args.frame_id + marker.header.stamp = stamp + marker.ns = "actual_color_recipe_steps" + marker.id = marker_id + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose = pose(step.xyz) + marker.scale = Vector3(x=step.scale, y=step.scale, z=step.scale) + marker.color.r, marker.color.g, marker.color.b, marker.color.a = step.color + return marker + + def text_marker(self, marker_id: int, step: Step, stamp) -> Marker: + marker = Marker() + marker.header.frame_id = self.args.frame_id + marker.header.stamp = stamp + marker.ns = "actual_color_recipe_labels" + marker.id = marker_id + marker.type = Marker.TEXT_VIEW_FACING + marker.action = Marker.ADD + marker.pose = pose((step.xyz[0], step.xyz[1], step.xyz[2] + 0.045)) + marker.scale.z = 0.032 + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 1.0 + marker.color.a = 1.0 + marker.text = step.label + return marker + + def command_text_marker(self, marker_id: int, stamp) -> Marker: + marker = Marker() + marker.header.frame_id = self.args.frame_id + marker.header.stamp = stamp + marker.ns = "actual_color_recipe_command" + marker.id = marker_id + marker.type = Marker.TEXT_VIEW_FACING + marker.action = Marker.ADD + marker.pose = pose((0.43, 0.20, 0.62)) + marker.scale.z = 0.04 + marker.color.r = 0.1 + marker.color.g = 1.0 + marker.color.b = 0.3 + marker.color.a = 1.0 + marker.text = ( + "RViz mirror: run_color_recipe_sequence.py --execute --confirm | " + f"sequence={','.join(self.sequence)}" + ) + return marker + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--colors", default="") + parser.add_argument("--dispenser-ids", default="") + parser.add_argument("--color-map-json", default="") + parser.add_argument("--allow-missing-color-map-fallback", action="store_true") + parser.add_argument("--frame-id", default="base_link") + parser.add_argument("--dispenser-config", type=Path, default=DEFAULT_DISPENSER_CONFIG) + parser.add_argument("--calibration", type=Path, default=DEFAULT_CALIBRATION) + parser.add_argument("--publish-rate-hz", type=float, default=5.0) + parser.add_argument("--move-prehold-offset-x-m", type=float, default=-0.030) + 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.180) + parser.add_argument("--move-release-offset-x-m", type=float, default=0.0) + parser.add_argument("--move-release-offset-y-m", type=float, default=0.0) + 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.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) + parser.add_argument("--regrasp-min-transit-z-m", type=float, default=0.500) + parser.add_argument("--regrasp-approach-offset-z-m", type=float, default=0.250) + parser.add_argument("--regrasp-max-transit-z-m", type=float, default=0.560) + parser.add_argument("--regrasp-retreat-x-m", type=float, default=-0.080) + parser.add_argument("--regrasp-retreat-y-m", type=float, default=0.0) + parser.add_argument("--regrasp-rear-entry-offset-x-m", type=float, default=-0.080) + parser.add_argument("--regrasp-rear-entry-offset-y-m", type=float, default=0.0) + parser.add_argument("--pick-lift-m", type=float, default=0.100) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + try: + sequence, color_map, color_pumps = derive_sequence(args) + steps = build_steps(args, sequence) + except Exception as exc: + print(f"[FAIL] cannot build RViz preview: {exc}", file=sys.stderr) + return 1 + + print("[Azas] RViz mirror for exact color recipe command") + print("[Azas] no robot/gripper/camera services will be called") + print(f"[Azas] color_map={color_map if color_map else 'direct dispenser input'}") + if color_pumps: + print(f"[Azas] recipe colors+pumps={color_pumps}") + print(f"[Azas] dispenser_ids={','.join(sequence)}") + print("[Azas] publishing /azas/dispenser_sequence/plan and /azas/dispenser_sequence/markers") + + rclpy.init() + node = ColorRecipePreviewPublisher(args, steps, sequence, color_map, color_pumps) + try: + rclpy.spin(node) + except (ExternalShutdownException, KeyboardInterrupt): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/publish_measured_recipe_joint_rviz_preview.py b/tools/run/publish_measured_recipe_joint_rviz_preview.py new file mode 100755 index 0000000..df90649 --- /dev/null +++ b/tools/run/publish_measured_recipe_joint_rviz_preview.py @@ -0,0 +1,300 @@ +#!/usr/bin/env python3 +"""RViz-only measured joint preview for the cocktail dispenser recipe. + +This publishes /joint_states from calibration.yaml DISP/PRESS joint pairs. It +does not call Doosan motion, gripper, camera, or execution services. +""" + +from __future__ import annotations + +import argparse +import json +import math +import sys +from dataclasses import dataclass +from pathlib import Path + +import rclpy +import yaml +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from sensor_msgs.msg import JointState +from visualization_msgs.msg import Marker, MarkerArray + +ROOT = Path(__file__).resolve().parents[2] +RUN_DIR = ROOT / "tools" / "run" +sys.path.insert(0, str(RUN_DIR)) + +from run_color_recipe_sequence import ( # noqa: E402 + RECIPE_PATH, + color_to_dispenser_id, + load_color_map, + parse_colors_arg, + parse_direct_dispenser_sequence, + parse_recipe_data, +) + +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} + +CALIBRATION = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +JOINT_NAMES = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + + +@dataclass(frozen=True) +class Keyframe: + label: str + joints_deg: list[float] + hold_frames: int = 8 + + +def read_yaml(path: Path) -> dict: + data = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + if not isinstance(data, dict): + raise ValueError(f"YAML is not a map: {path}") + return data + + +def numeric_list(raw: object, label: str, size: int) -> list[float]: + if not isinstance(raw, list) or len(raw) != size: + raise ValueError(f"{label} must be a {size}-item list") + return [float(value) for value in raw] + + +def group_consecutive(ids: list[str]) -> list[tuple[str, int]]: + grouped: list[tuple[str, int]] = [] + for dispenser_id in ids: + if grouped and grouped[-1][0] == dispenser_id: + grouped[-1] = (dispenser_id, grouped[-1][1] + 1) + else: + grouped.append((dispenser_id, 1)) + return grouped + + +def derive_sequence(args: argparse.Namespace) -> tuple[list[str], dict[str, str], list[tuple[str, int]]]: + if args.dispenser_ids.strip(): + return parse_direct_dispenser_sequence(args.dispenser_ids), {}, [] + + color_map = load_color_map(override_json=args.color_map_json) + if not color_map: + raise RuntimeError("color map is missing/invalid; use --dispenser-ids for physical-number preview") + + if args.colors.strip(): + color_pumps = parse_colors_arg(args.colors) + else: + if not RECIPE_PATH.exists(): + raise RuntimeError(f"recipe is missing: {RECIPE_PATH}") + color_pumps = parse_recipe_data(json.loads(RECIPE_PATH.read_text(encoding="utf-8"))) + + sequence: list[str] = [] + for color, count in color_pumps: + dispenser_id = color_to_dispenser_id(color, color_map) + if dispenser_id is None: + raise RuntimeError(f"color {color!r} is not present in color map {color_map}") + sequence.extend([dispenser_id] * count) + if not sequence: + raise RuntimeError("derived dispenser sequence is empty") + return sequence, color_map, color_pumps + + +def load_outlet_joints(calibration: Path, dispenser_id: str) -> dict[str, list[float]]: + data = read_yaml(calibration) + outlet = (data.get("dispenser_outlets") or {}).get(dispenser_id) + if not isinstance(outlet, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing in {calibration}") + status = str(outlet.get("press_contact_status", "")).strip() + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + raise ValueError( + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg is marked " + f"{status!r}; refusing to preview stale PRESS{dispenser_id}_CONTACT" + ) + return { + "cup_pre": numeric_list(outlet.get("cup_pre_place_joints_deg"), f"D{dispenser_id} cup_pre_place_joints_deg", 6), + "cup_place": numeric_list(outlet.get("cup_place_joints_deg"), f"D{dispenser_id} cup_place_joints_deg", 6), + "press_contact": numeric_list(outlet.get("press_contact_joints_deg"), f"D{dispenser_id} press_contact_joints_deg", 6), + } + + +def build_keyframes(args: argparse.Namespace, sequence: list[str]) -> list[Keyframe]: + keyframes: list[Keyframe] = [] + if args.include_home: + keyframes.append(Keyframe("HOME start", [0.0, 0.0, 90.0, 0.0, 90.0, 0.0], args.hold_frames)) + + for group_index, (dispenser_id, count) in enumerate(group_consecutive(sequence), start=1): + joints = load_outlet_joints(args.calibration, dispenser_id) + prefix = f"G{group_index} D{dispenser_id}x{count}" + if args.preview_mode == "press-only": + keyframes.append( + Keyframe( + f"{prefix} PRESS_CONTACT measured; real PRE=CONTACT+Z, PRESS=CONTACT-Z", + joints["press_contact"], + args.hold_frames, + ) + ) + for press_index in range(1, count + 1): + suffix = f"{press_index}/{count}" if count > 1 else "1/1" + keyframes.append( + Keyframe(f"{prefix} PRESS_CONTACT touch {suffix}", joints["press_contact"], args.press_hold_frames) + ) + keyframes.append( + Keyframe( + f"{prefix} PRESS_EXTRA_Z visual note {suffix}", + joints["press_contact"], + args.press_hold_frames, + ) + ) + keyframes.append( + Keyframe( + f"{prefix} GENERATED_PRESS_PRE visual note {suffix}", + joints["press_contact"], + args.hold_frames, + ) + ) + continue + + keyframes.extend( + [ + Keyframe(f"{prefix} DISP_PRE cup approach", joints["cup_pre"], args.hold_frames), + Keyframe(f"{prefix} DISP_PLACE cup release", joints["cup_place"], args.release_hold_frames), + Keyframe(f"{prefix} SAFE after release: RG2 open then close empty", joints["cup_pre"], args.hold_frames), + Keyframe( + f"{prefix} PRESS_CONTACT measured; real PRE=CONTACT+Z", + joints["press_contact"], + args.hold_frames, + ), + ] + ) + for press_index in range(1, count + 1): + suffix = f"{press_index}/{count}" if count > 1 else "1/1" + keyframes.append(Keyframe(f"{prefix} PRESS_CONTACT touch {suffix}", joints["press_contact"], args.press_hold_frames)) + keyframes.append(Keyframe(f"{prefix} PRESS_EXTRA_Z visual note {suffix}", joints["press_contact"], args.press_hold_frames)) + keyframes.append(Keyframe(f"{prefix} GENERATED_PRESS_PRE visual note {suffix}", joints["press_contact"], args.hold_frames)) + keyframes.extend( + [ + Keyframe(f"{prefix} SAFE robot-side retreat: RG2 opens here", joints["press_contact"], args.release_hold_frames), + Keyframe(f"{prefix} DISP_PRE re-grasp approach with RG2 already open", joints["cup_pre"], args.hold_frames), + Keyframe(f"{prefix} DISP_PLACE side grasp", joints["cup_place"], args.release_hold_frames), + Keyframe(f"{prefix} DISP_PRE lift after grasp", joints["cup_pre"], args.hold_frames), + ] + ) + return keyframes + + +def interpolate_deg(start: list[float], end: list[float], ratio: float) -> list[float]: + smooth = 0.5 - 0.5 * math.cos(math.pi * max(0.0, min(ratio, 1.0))) + return [a + (b - a) * smooth for a, b in zip(start, end)] + + +class MeasuredJointPreview(Node): + def __init__(self, args: argparse.Namespace, keyframes: list[Keyframe], sequence: list[str]) -> None: + super().__init__("azas_measured_recipe_joint_rviz_preview") + self.args = args + self.keyframes = keyframes + self.sequence = sequence + self.segment = 0 + self.frame = 0 + self.js_pub = self.create_publisher(JointState, args.joint_state_topic, 10) + self.marker_pub = self.create_publisher(MarkerArray, args.marker_topic, 10) + self.create_timer(1.0 / max(args.publish_rate_hz, 1.0), self.publish) + self.get_logger().info("RViz-only measured joint preview publishing; no robot services are called") + self.get_logger().info(f"sequence={','.join(sequence)} grouped={group_consecutive(sequence)}") + + def publish(self) -> None: + if not self.keyframes: + return + start = self.keyframes[self.segment] + end = self.keyframes[min(self.segment + 1, len(self.keyframes) - 1)] + frames = max(self.args.frames_per_segment, 1) + ratio = self.frame / frames + joints = interpolate_deg(start.joints_deg, end.joints_deg, ratio) + + stamp = self.get_clock().now().to_msg() + msg = JointState() + msg.header.stamp = stamp + msg.name = JOINT_NAMES + msg.position = [math.radians(value) for value in joints] + self.js_pub.publish(msg) + self.marker_pub.publish(self.make_label_marker(stamp, end.label, joints)) + + self.frame += 1 + if self.frame > frames + max(end.hold_frames, 0): + self.frame = 0 + self.segment += 1 + if self.segment >= len(self.keyframes) - 1: + self.segment = 0 if self.args.loop else len(self.keyframes) - 2 + + def make_label_marker(self, stamp, label: str, joints: list[float]) -> MarkerArray: + clear = Marker() + clear.header.frame_id = self.args.frame_id + clear.header.stamp = stamp + clear.action = Marker.DELETEALL + + text = Marker() + text.header.frame_id = self.args.frame_id + text.header.stamp = stamp + text.ns = "measured_joint_recipe_preview" + text.id = 1 + text.type = Marker.TEXT_VIEW_FACING + text.action = Marker.ADD + text.pose.position.x = 0.48 + text.pose.position.y = -0.36 + text.pose.position.z = 0.72 + text.pose.orientation.w = 1.0 + text.scale.z = 0.035 + text.color.r = 1.0 + text.color.g = 1.0 + text.color.b = 1.0 + text.color.a = 1.0 + text.text = f"{label}\nsequence={','.join(self.sequence)}\njoints_deg={[round(v, 1) for v in joints]}" + return MarkerArray(markers=[clear, text]) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Preview measured dispenser recipe joints in RViz only.") + parser.add_argument("--colors", default="") + parser.add_argument("--dispenser-ids", default="") + parser.add_argument("--color-map-json", default="") + parser.add_argument("--calibration", type=Path, default=CALIBRATION) + parser.add_argument("--frame-id", default="base_link") + parser.add_argument("--joint-state-topic", default="/joint_states") + parser.add_argument("--marker-topic", default="/azas/measured_joint_preview/markers") + parser.add_argument("--publish-rate-hz", type=float, default=30.0) + parser.add_argument("--frames-per-segment", type=int, default=45) + parser.add_argument("--hold-frames", type=int, default=12) + parser.add_argument("--release-hold-frames", type=int, default=30) + parser.add_argument("--press-hold-frames", type=int, default=18) + parser.add_argument("--loop", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--include-home", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--preview-mode", choices=["full", "press-only"], default="full") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + sequence, color_map, color_pumps = derive_sequence(args) + print(f"[Azas] RViz measured joint preview sequence={','.join(sequence)} grouped={group_consecutive(sequence)}") + if color_map: + print(f"[Azas] color_map={color_map}") + print(f"[Azas] color_pumps={color_pumps}") + keyframes = build_keyframes(args, sequence) + print(f"[Azas] keyframes={len(keyframes)}") + rclpy.init() + node = MeasuredJointPreview(args, keyframes, sequence) + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/record_dispenser_press_pose.py b/tools/run/record_dispenser_press_pose.py new file mode 100644 index 0000000..d0f8390 --- /dev/null +++ b/tools/run/record_dispenser_press_pose.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python3 +"""Record the current robot pose as a measured dispenser press teach point. + +This helper commands no motion. The operator must jog/teach the real robot to +the intended dispenser pre/contact pose first; this script only records the +current Doosan services into calibration.yaml. +""" + +from __future__ import annotations + +import argparse +import math +import re +import shutil +import sys +from datetime import datetime +from pathlib import Path +from typing import Any + +import rclpy +from dsr_msgs2.srv import GetCurrentPosj, GetCurrentPosx + +ROOT = Path("/home/ssu/Azas") +DEFAULT_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +CONFIRM = "ENABLE_RECORD_DISPENSER_PRESS_POSE" + + +def service_name(prefix: str, suffix: str) -> str: + clean_prefix = prefix.strip("/") + clean_suffix = suffix.strip("/") + return f"/{clean_prefix}/{clean_suffix}" if clean_prefix else f"/{clean_suffix}" + + +def call_service(node: Any, client: Any, request: Any, *, timeout_sec: float, label: str) -> Any: + if not client.wait_for_service(timeout_sec=max(timeout_sec, 0.1)): + raise RuntimeError(f"{label} service unavailable: {client.srv_name}") + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=max(timeout_sec, 0.1)) + if not future.done(): + raise RuntimeError(f"{label} timed out after {timeout_sec:.1f}s") + if future.exception() is not None: + raise RuntimeError(f"{label} exception: {future.exception()}") + result = future.result() + if result is None: + raise RuntimeError(f"{label} returned no result") + return result + + +def read_current_pose(service_prefix: str, timeout_sec: float) -> tuple[list[float], list[float]]: + rclpy.init(args=None) + node = rclpy.create_node("azas_record_dispenser_press_pose") + try: + posx_client = node.create_client(GetCurrentPosx, service_name(service_prefix, "aux_control/get_current_posx")) + posj_client = node.create_client(GetCurrentPosj, service_name(service_prefix, "aux_control/get_current_posj")) + + posx_req = GetCurrentPosx.Request() + posx_req.ref = 0 + posx_res = call_service(node, posx_client, posx_req, timeout_sec=timeout_sec, label="GetCurrentPosx") + if not posx_res.success or not posx_res.task_pos_info: + raise RuntimeError("GetCurrentPosx returned success=false or empty task_pos_info") + posx = [float(value) for value in list(posx_res.task_pos_info[0].data)[:6]] + if len(posx) != 6: + raise RuntimeError(f"GetCurrentPosx returned too few values: {posx}") + + posj_res = call_service(node, posj_client, GetCurrentPosj.Request(), timeout_sec=timeout_sec, label="GetCurrentPosj") + posj = [float(value) for value in list(posj_res.pos)[:6]] + if not posj_res.success or len(posj) != 6: + raise RuntimeError(f"GetCurrentPosj returned success=false or too few values: {posj}") + return posx, posj + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +def format_list(values: list[float], precision: int) -> str: + return "[" + ", ".join(f"{value:.{precision}f}" for value in values) + "]" + + +def replace_block_value(text: str, dispenser_id: str, key: str, value: str, comment: str = "") -> str: + header_pattern = re.compile(rf'^ "{re.escape(dispenser_id)}":\n', re.M) + match = header_pattern.search(text) + if not match: + raise RuntimeError(f'dispenser_outlets."{dispenser_id}" block not found') + next_match = re.search(r'^(?: "\d+":|[A-Za-z_][A-Za-z0-9_]*:)\n', text[match.end():], re.M) + block_end = match.end() + next_match.start() if next_match else len(text) + block = text[match.end():block_end] + line_pattern = re.compile(rf"^ {re.escape(key)}: .*$", re.M) + replacement = f" {key}: {value}{comment}" + if line_pattern.search(block): + block = line_pattern.sub(replacement, block, count=1) + else: + block = block.rstrip() + "\n" + replacement + "\n" + return text[:match.end()] + block + text[block_end:] + + +def update_config(config_path: Path, dispenser_id: str, kind: str, posx: list[float], posj: list[float]) -> Path: + stamp = datetime.now().strftime("%Y%m%d-%H%M%S") + backup_path = config_path.with_suffix(config_path.suffix + f".bak-{stamp}") + shutil.copy2(config_path, backup_path) + + text = config_path.read_text(encoding="utf-8") + measured_comment = f" # operator measured, {datetime.now().date().isoformat()}" + if kind == "contact": + xyz_m = [value / 1000.0 for value in posx[:3]] + rpy_deg = posx[3:6] + rpy_rad = [math.radians(value) for value in rpy_deg] + text = replace_block_value(text, dispenser_id, "press_pose_xyz_m", format_list(xyz_m, 6)) + text = replace_block_value(text, dispenser_id, "press_pose_rpy_deg", format_list(rpy_deg, 3)) + text = replace_block_value(text, dispenser_id, "press_pose_rpy_rad", format_list(rpy_rad, 6)) + text = replace_block_value( + text, + dispenser_id, + "press_contact_joints_deg", + format_list(posj, 2), + measured_comment, + ) + text = replace_block_value( + text, + dispenser_id, + "press_contact_status", + "measured_confirmed", + measured_comment, + ) + elif kind == "pre": + text = replace_block_value( + text, + dispenser_id, + "press_pre_joints_deg", + format_list(posj, 2), + measured_comment, + ) + elif kind == "common_pre": + text = replace_block_value( + text, + dispenser_id, + "press_common_pre_joints_deg", + format_list(posj, 2), + measured_comment, + ) + elif kind == "cup_common_pre": + text = replace_block_value( + text, + dispenser_id, + "cup_common_pre_joints_deg", + format_list(posj, 2), + measured_comment, + ) + elif kind == "cup_pre": + text = replace_block_value( + text, + dispenser_id, + "cup_pre_place_joints_deg", + format_list(posj, 2), + measured_comment, + ) + elif kind == "cup_place": + text = replace_block_value( + text, + dispenser_id, + "cup_place_joints_deg", + format_list(posj, 2), + measured_comment, + ) + text = replace_block_value( + text, + dispenser_id, + "cup_place_status", + "measured_confirmed", + measured_comment, + ) + else: + raise RuntimeError(f"unsupported kind: {kind}") + + config_path.write_text(text, encoding="utf-8") + return backup_path + + +def warn_lane_mismatch(dispenser_id: str, kind: str, posx: list[float], tolerance_mm: float = 25.0) -> None: + """Warn when the recorded pose sits laterally over a different dispenser lane. + + Guards against the slot mix-ups observed on 2026-06-10 where press teach + poses were saved under neighboring dispenser ids. Warning only; the + operator decides. + """ + collision_config = ROOT / "src" / "azas_bringup" / "config" / "measured_dispenser_collision.yaml" + try: + import yaml + + data = yaml.safe_load(collision_config.read_text(encoding="utf-8")) or {} + front_holds = data.get("front_hold_poses") or {} + lane_y_mm = { + key.split("_")[-1]: float(value["position_xyz_m"][1]) * 1000.0 + for key, value in front_holds.items() + } + except Exception: + return + expected = lane_y_mm.get(dispenser_id) + if expected is None: + return + actual = posx[1] + delta = actual - expected + nearest = min(lane_y_mm.items(), key=lambda kv: abs(kv[1] - actual)) + if abs(delta) > tolerance_mm: + print( + f"[WARN] {kind} y={actual:.1f}mm is {abs(delta):.0f}mm away from dispenser " + f"{dispenser_id} lane (expected y~{expected:.1f}mm); nearest lane is " + f"dispenser {nearest[0]}. Check the dispenser id before trusting this record." + ) + else: + print(f"[Azas] lane check OK: y={actual:.1f}mm matches dispenser {dispenser_id} (expected ~{expected:.1f}mm)") + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--dispenser-id", required=True, choices=["1", "2", "3", "4"]) + parser.add_argument( + "--kind", + required=True, + choices=["pre", "common_pre", "contact", "cup_common_pre", "cup_pre", "cup_place"], + ) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) + parser.add_argument("--timeout-sec", type=float, default=8.0) + parser.add_argument("--write", action="store_true") + parser.add_argument("--confirm", default="") + args = parser.parse_args() + + if args.write and args.confirm != CONFIRM: + print(f"[BLOCKED] --write requires --confirm {CONFIRM}", file=sys.stderr) + return 2 + if not args.config.is_file(): + print(f"[FAIL] config not found: {args.config}", file=sys.stderr) + return 2 + + if not args.write: + print(f"[BLOCKED] --write --confirm {CONFIRM} is required; no dry-run recording mode is allowed.", file=sys.stderr) + return 2 + + posx, posj = read_current_pose(args.service_prefix, args.timeout_sec) + print(f"[Azas] dispenser={args.dispenser_id} kind={args.kind}") + print(f"[Azas] current_posx_mm_deg={format_list(posx, 3)}") + print(f"[Azas] current_posj_deg={format_list(posj, 2)}") + if args.kind == "contact": + print(f"[Azas] calibration press_pose_xyz_m={format_list([value / 1000.0 for value in posx[:3]], 6)}") + print(f"[Azas] calibration press_pose_rpy_deg={format_list(posx[3:6], 3)}") + print(f"[Azas] calibration press_contact_joints_deg={format_list(posj, 2)}") + elif args.kind == "pre": + print(f"[Azas] calibration press_pre_joints_deg={format_list(posj, 2)}") + elif args.kind == "common_pre": + print(f"[Azas] calibration press_common_pre_joints_deg={format_list(posj, 2)}") + elif args.kind == "cup_common_pre": + print(f"[Azas] calibration cup_common_pre_joints_deg={format_list(posj, 2)}") + elif args.kind == "cup_pre": + print(f"[Azas] calibration cup_pre_place_joints_deg={format_list(posj, 2)}") + else: + print(f"[Azas] calibration cup_place_joints_deg={format_list(posj, 2)}") + warn_lane_mismatch(args.dispenser_id, args.kind, posx) + + backup_path = update_config(args.config, args.dispenser_id, args.kind, posx, posj) + print(f"[PASS] updated {args.config}") + print(f"[Azas] backup={backup_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) 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/replay_registered_press_poses.py b/tools/run/replay_registered_press_poses.py new file mode 100755 index 0000000..a9a05d4 --- /dev/null +++ b/tools/run/replay_registered_press_poses.py @@ -0,0 +1,318 @@ +#!/usr/bin/env python3 +"""Replay registered dispenser press teach poses one by one for visual lane checks. + +Reads press_pre_joints_deg / press_contact_joints_deg for each dispenser from +calibration.yaml, prints an offline URDF-FK prediction of which pump lane each +pose is over, then (with --execute) drives the robot slowly to each pose with a +safe joint-home transit between poses so a mislabeled contact pose cannot drag +across pump heads. + +Usage: + python3 tools/run/replay_registered_press_poses.py # plan only + python3 tools/run/replay_registered_press_poses.py --rviz # RViz preview + python3 tools/run/replay_registered_press_poses.py --execute \ + --confirm ENABLE_PRESS_POSE_REPLAY # real motion + +RViz preview mode publishes /joint_states (joint_1..joint_6) plus markers for +the four configured press_pose_xyz_m lanes. Bring up the hardware-free scene +first: + ros2 launch azas_bringup hardware_free_demo.launch.py + +The operator watches which pump each pose actually hovers over and notes the +true lane; afterwards the YAML slots are rearranged to match reality. +""" + +from __future__ import annotations + +import argparse +import math +import subprocess +import sys +import xml.etree.ElementTree as ET +from pathlib import Path + +import numpy as np +import yaml + +ROOT = Path(__file__).resolve().parents[2] +CALIBRATION = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +URDF = ROOT / "install" / "dsr_description2" / "share" / "dsr_description2" / "urdf" / "m0609.urdf" +MOVEJ_TOOL = ROOT / "tools" / "run" / "direct_movej_joints.py" +CONFIRM_PHRASE = "ENABLE_PRESS_POSE_REPLAY" +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} +SAFE_HOME_DEG = [0.0, 0.0, 90.0, 0.0, 90.0, 0.0] + + +def load_chain(): + tree = ET.parse(URDF) + joints = {} + for j in tree.getroot().findall("joint"): + if j.get("type") not in ("revolute", "fixed"): + continue + origin = j.find("origin") + xyz = [float(v) for v in (origin.get("xyz") or "0 0 0").split()] if origin is not None else [0.0, 0.0, 0.0] + rpy = [float(v) for v in (origin.get("rpy") or "0 0 0").split()] if origin is not None else [0.0, 0.0, 0.0] + axis_el = j.find("axis") + axis = [float(v) for v in (axis_el.get("xyz") or "0 0 1").split()] if axis_el is not None else [0.0, 0.0, 1.0] + joints[j.get("name")] = dict( + parent=j.find("parent").get("link"), + child=j.find("child").get("link"), + xyz=xyz, + rpy=rpy, + axis=axis, + type=j.get("type"), + ) + chain = [] + link = "link_6" + while True: + entry = next(((n, j) for n, j in joints.items() if j["child"] == link), None) + if entry is None: + break + chain.append(entry) + link = entry[1]["parent"] + if link in ("base_link", "base", "world", "base_0"): + break + chain.reverse() + return chain + + +def rpy_mat(r: float, p: float, y: float) -> np.ndarray: + cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y) + rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) + ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) + rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) + return rz @ ry @ rx + + +def axis_rot(axis: list[float], theta: float) -> np.ndarray: + a = np.array(axis) / np.linalg.norm(axis) + k = np.array([[0, -a[2], a[1]], [a[2], 0, -a[0]], [-a[1], a[0], 0]]) + return np.eye(3) + math.sin(theta) * k + (1 - math.cos(theta)) * (k @ k) + + +def flange_fk_mm(chain, q_deg: list[float]) -> np.ndarray: + t = np.eye(4) + qi = 0 + for _, j in chain: + a = np.eye(4) + a[:3, :3] = rpy_mat(*j["rpy"]) + a[:3, 3] = j["xyz"] + t = t @ a + if j["type"] == "revolute": + r = np.eye(4) + r[:3, :3] = axis_rot(j["axis"], math.radians(q_deg[qi])) + qi += 1 + t = t @ r + return t[:3, 3] * 1000.0 + + +def lane_guess(flange_y_mm: float) -> str: + # Flange-frame lane centers observed for this setup (outlet TCP y 84/43/-2/-50). + lanes = {"1": 71.0, "2": 27.0, "3": -33.0, "4": -68.0} + best = min(lanes.items(), key=lambda kv: abs(kv[1] - flange_y_mm)) + return f"{best[0]}번 레인 부근 (오차 {abs(best[1] - flange_y_mm):.0f}mm)" + + +def movej(joints_deg: list[float], *, service_prefix: str, velocity: float, label: str) -> None: + cmd = [ + sys.executable, + str(MOVEJ_TOOL), + "--service-prefix", service_prefix, + "--j1", str(joints_deg[0]), "--j2", str(joints_deg[1]), "--j3", str(joints_deg[2]), + "--j4", str(joints_deg[3]), "--j5", str(joints_deg[4]), "--j6", str(joints_deg[5]), + "--velocity", str(velocity), "--acceleration", str(velocity), + "--j5-min-deg", "-150", "--j5-max-deg", "150", + "--timeout-sec", "60", "--motion-timeout-sec", "120", + "--execute", "--confirm", "ENABLE_DIRECT_MOVEJ", + ] + print(f"[replay] movej: {label}") + result = subprocess.run(cmd, check=False) + if result.returncode != 0: + raise RuntimeError(f"movej failed for {label} (rc={result.returncode}); aborting replay") + + +def run_rviz_preview(poses: list[tuple[str, list[float]]], chain, outlets: dict) -> int: + import threading + import time + + import rclpy + from rclpy.node import Node as RclpyNode + from sensor_msgs.msg import JointState + from visualization_msgs.msg import Marker, MarkerArray + + rclpy.init() + node = RclpyNode("press_pose_replay_rviz_preview") + joint_pub = node.create_publisher(JointState, "/joint_states", 10) + marker_pub = node.create_publisher(MarkerArray, "/azas/press_pose_replay/markers", 10) + + current = {"q": list(poses[0][1]) if poses else [0.0] * 6, "target": None, "label": ""} + lock = threading.Lock() + + def make_markers() -> MarkerArray: + markers = MarkerArray() + for index, did in enumerate(("1", "2", "3", "4")): + block = outlets.get(did) or {} + xyz = block.get("press_pose_xyz_m") + if not xyz: + continue + sphere = Marker() + sphere.header.frame_id = "base_link" + sphere.ns = "press_pose" + sphere.id = index + sphere.type = Marker.SPHERE + sphere.action = Marker.ADD + sphere.pose.position.x = float(xyz[0]) + sphere.pose.position.y = float(xyz[1]) + sphere.pose.position.z = float(xyz[2]) + sphere.pose.orientation.w = 1.0 + sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.03 + sphere.color.r, sphere.color.g, sphere.color.b, sphere.color.a = 1.0, 0.3, 0.1, 0.9 + markers.markers.append(sphere) + text = Marker() + text.header.frame_id = "base_link" + text.ns = "press_pose_label" + text.id = index + text.type = Marker.TEXT_VIEW_FACING + text.action = Marker.ADD + text.pose.position.x = float(xyz[0]) + text.pose.position.y = float(xyz[1]) + text.pose.position.z = float(xyz[2]) + 0.05 + text.pose.orientation.w = 1.0 + text.scale.z = 0.04 + text.color.r = text.color.g = text.color.b = text.color.a = 1.0 + text.text = f"press {did}" + markers.markers.append(text) + return markers + + def spin_loop() -> None: + rate_sec = 1.0 / 30.0 + step_per_tick = math.radians(20.0) * rate_sec # 20 deg/s preview speed + while rclpy.ok(): + with lock: + target = current["target"] + if target is not None: + done = True + for i in range(6): + delta = math.radians(target[i]) - current["q"][i] + if abs(delta) > step_per_tick: + current["q"][i] += math.copysign(step_per_tick, delta) + done = False + else: + current["q"][i] = math.radians(target[i]) + if done: + current["target"] = None + msg = JointState() + msg.header.stamp = node.get_clock().now().to_msg() + msg.name = [f"joint_{i}" for i in range(1, 7)] + msg.position = list(current["q"]) + joint_pub.publish(msg) + marker_pub.publish(make_markers()) + time.sleep(rate_sec) + + with lock: + current["q"] = [math.radians(v) for v in (poses[0][1] if poses else [0.0] * 6)] + thread = threading.Thread(target=spin_loop, daemon=True) + thread.start() + + print("\n[RViz] /joint_states 퍼블리시 중. hardware_free_demo.launch.py RViz에서 로봇이 보여야 합니다.") + print("[RViz] 빨간 구슬 = calibration의 press_pose_xyz_m (목표 레인). 로봇 그리퍼가 어느 구슬 위인지 비교하세요.") + for index, (name, joints) in enumerate(poses, start=1): + xyz = flange_fk_mm(chain, joints) + print(f"\n[RViz] ({index}/{len(poses)}) 슬롯 {name} → FK 예상 {lane_guess(xyz[1])}") + answer = input(" Enter=이 포즈로 미리보기 이동 / s=건너뜀 / q=종료: ").strip().lower() + if answer == "q": + break + if answer == "s": + continue + with lock: + current["target"] = list(joints) + input(f" >> RViz에서 {name} 자세 확인 후 Enter: ") + rclpy.shutdown() + return 0 + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--execute", action="store_true", help="실제 로봇 모션 실행") + parser.add_argument("--rviz", action="store_true", help="실로봇 대신 RViz 조인트 프리뷰로 재생") + parser.add_argument("--confirm", default="", help=f"실행 시 {CONFIRM_PHRASE} 필요") + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--velocity", type=float, default=10.0) + parser.add_argument("--skip-home-between", action="store_true", + help="포즈 사이 조인트 홈 경유 생략 (권장하지 않음)") + parser.add_argument("--only", default="", help="예: 1_pre,3_contact 처럼 일부만 재생") + args = parser.parse_args() + + if args.execute and args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --execute에는 --confirm {CONFIRM_PHRASE} 가 필요합니다.", file=sys.stderr) + return 2 + + data = yaml.safe_load(CALIBRATION.read_text(encoding="utf-8")) or {} + outlets = data.get("dispenser_outlets") or {} + chain = load_chain() + + poses: list[tuple[str, list[float]]] = [] + for did in ("1", "2", "3", "4"): + block = outlets.get(did) or {} + for kind, key in (("pre", "press_pre_joints_deg"), ("contact", "press_contact_joints_deg")): + joints = block.get(key) + if joints is None: + continue + if kind == "contact": + status = str(block.get("press_contact_status", "")).strip() + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + print( + f"[BLOCKED] {did}_contact is marked {status!r}; " + f"skipping stale PRESS{did}_CONTACT until re-taught" + ) + continue + poses.append((f"{did}_{kind}", [float(v) for v in joints])) + + selected = {token.strip() for token in args.only.split(",") if token.strip()} + if selected: + poses = [p for p in poses if p[0] in selected] + + print(f"[replay] calibration: {CALIBRATION}") + print(f"[replay] {len(poses)}개 포즈 재생 예정 (홈 경유 {'생략' if args.skip_home_between else '포함'})") + print(f"{'슬롯':12s} {'FK flange xyz(mm)':28s} 예상 레인") + for name, joints in poses: + xyz = flange_fk_mm(chain, joints) + print(f"{name:12s} [{xyz[0]:7.1f}, {xyz[1]:7.1f}, {xyz[2]:7.1f}] {lane_guess(xyz[1])}") + + if args.rviz: + return run_rviz_preview(poses, chain, outlets) + + if not args.execute: + print("\n[DRY-RUN] --execute 미지정: 모션 없음. 위 표로 예상 레인만 확인하세요. (--rviz로 시각 확인 가능)") + return 0 + + for index, (name, joints) in enumerate(poses, start=1): + xyz = flange_fk_mm(chain, joints) + print(f"\n[replay] ({index}/{len(poses)}) 슬롯 {name} → 예상 {lane_guess(xyz[1])}") + answer = input(" Enter=이동 / s=건너뜀 / q=종료: ").strip().lower() + if answer == "q": + break + if answer == "s": + continue + if not args.skip_home_between: + movej(SAFE_HOME_DEG, service_prefix=args.service_prefix, + velocity=args.velocity, label="safe joint home transit") + movej(joints, service_prefix=args.service_prefix, + velocity=args.velocity, label=f"registered pose {name}") + input(f" >> 지금 로봇이 실제로 몇 번 펌프 위에 있는지 기록하세요 ({name}). Enter=다음: ") + + if not args.skip_home_between: + movej(SAFE_HOME_DEG, service_prefix=args.service_prefix, + velocity=args.velocity, label="final safe joint home") + print("[replay] 완료. 기록한 실제 레인에 맞게 calibration.yaml 슬롯을 재배치하세요.") + return 0 + + +if __name__ == "__main__": + raise SystemExit(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/rg2_full_close_verify.sh b/tools/run/rg2_full_close_verify.sh index c5008dc..b7ffa4c 100755 --- a/tools/run/rg2_full_close_verify.sh +++ b/tools/run/rg2_full_close_verify.sh @@ -6,7 +6,7 @@ set -euo pipefail # Safety/validation note: # - Use this for dispenser pressing after the cup has already been released. # - Do NOT use this as the large-cup grasp command; use gripper_soft_grasp instead. -# - jarvis/rg2_trigger_node does not expose actual finger-position feedback. +# - azas_gripper/rg2_gripper_node does not expose actual finger-position feedback. # - This verifies the strongest available software evidence: full-close set_width # command was sent and the service returned success=True. diff --git a/tools/run/rg2_full_open_verify.sh b/tools/run/rg2_full_open_verify.sh index 9b3032d..02bb56b 100755 --- a/tools/run/rg2_full_open_verify.sh +++ b/tools/run/rg2_full_open_verify.sh @@ -4,7 +4,7 @@ set -euo pipefail # Command RG2 to a full-open target and verify the ROS service accepted it. # # Safety/validation note: -# - jarvis/rg2_trigger_node does not expose actual finger-position feedback. +# - azas_gripper/rg2_gripper_node does not expose actual finger-position feedback. # - This script verifies the strongest available software evidence: # the full-open set_width command was sent and the service returned success=True. # - Physical confirmation still requires watching the gripper or adding a feedback source. diff --git a/tools/run/rg2_set_width_verify.py b/tools/run/rg2_set_width_verify.py index 43d20c2..b10f03c 100755 --- a/tools/run/rg2_set_width_verify.py +++ b/tools/run/rg2_set_width_verify.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 """Send a SetGripper command with RG2 bridge readiness recovery. -The jarvis RG2 wrapper accepts Modbus write requests but does not expose actual +The Azas RG2 wrapper accepts Modbus write requests but does not expose actual finger-position feedback. This script therefore verifies the strongest software evidence available: the ROS service exists, returns success=True, and the command payload is logged. If the service is absent, it can start the RG2 bridge once and diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 5b506e1..96bc238 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -3,12 +3,15 @@ from __future__ import annotations +import errno import json 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 @@ -21,12 +24,23 @@ 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" 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:-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 && " @@ -34,14 +48,27 @@ f"source {shlex.quote(str(ROOT / 'install' / 'setup.bash'))}; " "else " f"source {shlex.quote(str(ROOT / 'install' / 'local_setup.bash'))}; " - "fi" + "fi && " + 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" +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" BACKGROUND_LOG_DIR = ROOT / "log" / "panel" +COMMAND_OVERRIDES_PATH = ROOT / "outputs" / "panel_command_overrides.json" ROBOT_STATE_NAMES = { 0: "STATE_INITIALIZING", 1: "STATE_STANDBY", @@ -58,18 +85,517 @@ } 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 = { +_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" +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]: + 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(): + 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() + + +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") + 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 _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 + 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 _recipe_color_pumps(recipe: Any) -> tuple[list[tuple[str, int]], list[str]]: + color_pumps: list[tuple[str, int]] = [] + issues: list[str] = [] + + def add(color: Any, count: Any = 1) -> None: + color_name = str(color).lower().strip() + if not color_name: + return + try: + pump_count = int(count) + except (TypeError, ValueError): + issues.append(f"invalid pump count for color: {color_name}") + return + if pump_count < 1: + issues.append(f"pump count must be >=1 for color: {color_name}") + return + color_pumps.append((color_name, pump_count)) + + if isinstance(recipe, list): + for item in recipe: + if isinstance(item, dict): + color = item.get("color") or item.get("name") + count = item.get("pumps", item.get("count", item.get("presses", 1))) + if color: + add(color, count) + else: + add(item, 1) + return color_pumps, issues + + if not isinstance(recipe, dict): + issues.append("recipe JSON must be an object or list") + return color_pumps, issues + + colors = recipe.get("colors") + pumps = None + for key in ("pumps", "presses", "counts"): + if key in recipe: + pumps = recipe.get(key) + break + if isinstance(colors, list): + if not isinstance(pumps, dict): + pumps = {} + for raw_color in colors: + color = str(raw_color).lower().strip() + add(color, pumps.get(color, pumps.get(str(raw_color), 1))) + return color_pumps, issues + + if isinstance(pumps, dict): + for color, count in pumps.items(): + add(color, count) + return color_pumps, issues + + metadata_keys = {"source", "note", "notes", "created_at", "updated_at"} + for color, count in recipe.items(): + if str(color).lower().strip() in metadata_keys: + continue + if isinstance(count, (int, float, str)): + add(color, count) + + if not color_pumps: + issues.append("recipe has no executable colors") + return color_pumps, issues + + +def _color_recipe_direct_arg(payload: dict[str, Any]) -> str: + recipe_override = str(payload.get("recipe_dispenser_ids") or "").strip() + if not recipe_override: + return "" + 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: + return f" --dispenser-ids {shlex.quote(recipe_override)}" + direct_color_map = json.dumps(_load_dispenser_press_targets(), ensure_ascii=False) + return ( + f" --colors {shlex.quote(recipe_override)}" + f" --color-map-json {shlex.quote(direct_color_map)}" + ) + + +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") + 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-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)}" + ) + + +def chain_recipe_after_manual_command(manual_cmd: str, payload: dict[str, Any], label: str) -> str: + recipe_cmd = color_recipe_sequence_command(payload) + return ( + f"( {manual_cmd} ); " + "manual_rc=$?; " + "if [ ${manual_rc} -eq 0 ]; then " + f"echo '[Azas] {label} 성공 메시지 확인 -> 통합 디스펜서 색상 레시피를 자동 실행합니다.'; " + "echo '[Azas] auto_integrated_dispenser_recipe=true'; " + f"{recipe_cmd}; " + "else " + f"echo '[Azas] {label} 실패/중단 rc='${{manual_rc}}' -> 디스펜서 레시피 실행을 건너뜁니다.'; " + "exit ${manual_rc}; " + "fi" + ) + + +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 ( + "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 tmux_stack_start_command(payload: dict[str, Any]) -> str: + 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} && " + "bash tools/run/stop_azas_all.sh && " + "sleep 2 && " + 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)} " + "bash tools/run/start_azas_tmux_stack.sh" + ) + + +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] = [] + 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: + 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), + "output_file": output_file, + "failed_file": failed_file, + "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), + "output_file": output_file, + "failed_file": failed_file, + "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 recipe is not None: + color_pumps, recipe_issues = _recipe_color_pumps(recipe) + issues.extend(recipe_issues) + for color, count in color_pumps: + dispenser_id = color_to_id.get(color) + if not dispenser_id: + issues.append(f"recipe color has no dispenser: {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), + "output_file": output_file, + "failed_file": failed_file, + "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") + 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) @@ -86,47 +612,183 @@ class Step: STEPS = [ Step( "connect_robot", - "로봇 연결 / 스마트 재연결", + "로봇 연결 / tmux 통합 재연결", + "background", + "tools/run/stop_azas_all.sh && sleep 2 && tools/run/start_azas_tmux_stack.sh", + True, + False, + "검증된 현장 명령으로 stop_azas_all 후 azas-logic tmux 스택을 시작", + ), + Step( + "start_tmux_stack", + "tmux 연결 스택 시작", "background", - "tools/run/run_doosan_real_no_motion_m0609.sh", + "tools/run/start_azas_tmux_stack.sh", True, False, - "준비됨/시작중이면 유지하고, stale 상태일 때만 정리 후 시작", + "검증된 tmux 방식으로 stop_azas_all 후 azas-logic 세션에 로봇, RG2 그리퍼, RealSense, joint relay를 분리 시작", + ), + Step( + "stop_azas_all", + "전체 정지 / ROS 정리", + "run", + "tools/run/stop_azas_all.sh", + True, + False, + "azas tmux 스택과 모든 ROS 노드/좀비 프로세스를 종료하고 FastDDS 공유메모리 잔여물(/dev/shm/fastrtps_*)을 정리. 패널/에이전트 프로세스는 보호됨. 정리 후 'tmux 연결 스택 시작'으로 재시작", ), 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 토픽 시작; 화면 창은 별도 버튼 사용"), + Step("start_camera_view", "RealSense 컬러 화면 보기", "background", "rqt_image_view /camera/camera/color/image_raw", True, False, "카메라 color image 토픽을 rqt_image_view 창으로 표시"), + Step("detect_cup_lid", "컵/뚜껑 탐지 토픽 시작", "background", "ros2 launch azas_bringup yolo_perception.launch.py", True, False, "YOLO 탐지 결과를 /azas/cup_detection으로 publish; 이 노드는 화면 창을 띄우지 않음"), 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, + "safety.yaml 바닥/양쪽 벽, measured dispenser 박스, link_6 부착 RG2 그리퍼 envelope, 감지 텀블러를 PlanningScene/RViz로 publish", + ), + 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, - "디스펜서 박스와 감지 텀블러를 /collision_object로 publish; direct Doosan 명령은 아직 이 장면을 자동 회피에 쓰지 않음", + "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", - "카메라 테이블 보기 자세 / 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("voice_input", "음성 입력", "run", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT/레시피 노드"), - Step("recipe_generate", "레시피 생성", "blocked", "", False, False, "음성/레시피 토픽 통합 버튼은 별도 연결 필요"), Step( - "side_grip", - "RealSense 컵 인식 후 side grip", + "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", + "색상 스캔 검증 포즈 이동 [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, + "색상 스캔 전 2026-06-08 검증 포즈로 이동. 카메라 화면에서 보이는 디스펜서 핸들 색을 직접 검출할 수 있는 자세", + ), + 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", + "디스펜서 색상 스캔", + "run", + "tools/run/dispenser_color_scan_ros.sh", + True, + False, + "카메라 화면의 visible colored handle blob을 직접 검출해 왼쪽→오른쪽을 디스펜서 1~4로 매핑하고 outputs/dispenser_color_map.json 저장. TF 투영은 보조 경로", + ), + 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초)", + "run", + "tools/run/listen_stt_recipe.py --timeout 60", + True, + False, + "사용자가 메뉴를 말하고 '응'으로 확정하면 /azas/voice/confirmed_recipe_decision 수신 → outputs/latest_recipe.json 저장", + ), + Step( + "run_color_recipe_sequence", + "통합 디스펜서 레시피 실행", "run", - "ros2 launch azas_bringup yolo_cup_pick_node_legacy.launch.py auto_pick:=true grasp_mode:=side use_measured_front_hold_pose:=true", + "tools/run/run_color_recipe_sequence.py --execute --confirm", True, True, - "shining-b-02 merged side-grip flow: RealSense color+aligned depth로 cup 탐지 후 선택 디스펜서의 측정 front_hold pose로 place", + "latest_recipe.json + dispenser_color_map.json → 컵 놓기→프레스→컵 다시 잡기/다음 디스펜서 이동. 재집기 Z 상승은 특이점 시 IK MoveJoint로 우회하고 legacy 저자세 직행 fallback은 사용하지 않음", + ), + Step( + "side_grip", + "PR #20 RealSense 컵 인식 후 side grip", + "background", + "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, y축 side-grip target 보정은 컵 방향 70mm", + ), + Step( + "cup_uprighting", + "소명 누운 컵 세우기 / cup uprighting", + "background", + "ros2 launch azas_cup_uprighting yolo_cup_uprighting.launch.py", + True, + True, + "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("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 배출구 아래로 컵 이동", @@ -165,39 +827,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 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), 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 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), 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 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), 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 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), Step( "pick_from_dispenser_1", @@ -245,7 +907,33 @@ class Step: "설정의 RECIPE_DISPENSER_IDS 순서대로 실행. 컵 이동/놓기와 다시 side-grip 집기는 통합 ROS 클라이언트로 처리해 반복 명령 실행 시간을 줄임", ), Step("repeat_dispense", "5,6 반복", "blocked", "", False, True, "레시피별 디스펜서 ID 반복 로직 필요"), - Step("pick_lid", "뚜껑을 집기", "blocked", "", False, True, "뚜껑 좌표/그리퍼 폭 필요"), + Step( + "pick_lid", + "뚜껑 grip pose 계획 / 빨간 스티커", + "background", + "", + True, + 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 DICT_6X6_250 id0 뚜껑 pose를 p키로 확정한 뒤 RG2 파지→lift→teach point 이동→J6 단계 회전으로 뚜껑을 닫음", + ), Step( "place_cup_holder", "컵을 컵홀더에 놓기 / side grip", @@ -255,7 +943,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 미리보기 / 무모션", @@ -265,15 +952,107 @@ class Step: False, "실제 로봇 미사용: 별도 ROS_DOMAIN_ID에서 쉐이킹 궤적/마커를 RViz로 표시", ), - Step("shake_closed_cup", "컵홀더 컵 다시 잡기 설명 후 쉐이킹", "run", "tools/run/run_rule_based_shake_real.sh", True, True, "시작 시 컵홀더에 놓인 닫힌 컵을 side grip으로 다시 잡은 상태/순서를 먼저 설명한 뒤, J3 양수 고정 및 J4/J5/J6 트위스트 쉐이킹을 실행"), - Step("remove_lid", "뚜껑을 열기/제거하기", "blocked", "", False, True, "뚜껑 제거 동작 미구현"), - Step("pour_cocktail", "칵테일을 다른 컵에 붓기", "blocked", "", False, True, "따르기 경로 미구현"), + 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", + "손 검출 시작 / 무모션", + "background", + "bash tools/run/run_human_hand_detection.sh", + True, + 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", + "쉐이킹 후 손바닥에 컵 건네기", + "run", + "tools/run/handover_cup_to_palm.py", + True, + True, + "실제모션 HRI: 손 검출이 먼저 켜져 있어야 함. 손바닥 위로 이동 후 외력 감시하며 저속 하강, 컵 release. " + "첫 사용 전 스펀지 테스트로 --release-tcp-above-palm-m 튜닝 필수", + ), ] 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 for long-lived +# panel jobs. The integrated reconnect command is intentionally excluded below: +# it runs stop_azas_all.sh, which kills azas-logic before recreating it, so that +# command must be launched by the panel server outside tmux. +PANEL_TMUX_SESSION = "azas-logic" +PANEL_TMUX_STEPS = { + "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", + "start_hand_detection", + "start_hand_detection_view", +} +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", + "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. + # 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", + "cup_uprighting", + "lid_grip_close", +} DOOSAN_STACK_PATTERNS = ( + "run_doosan_real_m0609.sh", "run_doosan_real_no_motion_m0609.sh", "run_emulator", "dsr_bringup2/lib/dsr_bringup2", @@ -283,6 +1062,10 @@ class Step: "ros2_control_node", "controller_manager", "joint_state_broadcaster", + "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", @@ -291,23 +1074,108 @@ class Step: ) AUXILIARY_STACK_PATTERNS = ( - "rg2_trigger", + "rg2_gripper_node", "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", + "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", "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", + "collision_scene_rviz_publisher", + "tumbler_collision_scene_node", + "link6_gripper_collision_node", + "rg2_link6_tcp.launch.py", + "azas_rg2_link6_tcp_state_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", + "collision_scene_rviz_publisher", "tumbler_collision_scene_node", + "link6_gripper_collision_node", + "rg2_link6_tcp.launch.py", + "azas_rg2_link6_tcp_state_publisher", + "--frame-id world --child-frame-id base_link", ) RG2_STACK_PATTERNS = ( - "rg2_trigger.launch.py", - "rg2_trigger_node", + "rg2_gripper_node", +) + +CAMERA_STACK_PATTERNS = ( + "realsense2_camera rs_launch.py", + "realsense2_camera_node", +) + +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", + "workspace_collision_scene_node", + "--frame-id world --child-frame-id base_link", +) + +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", + "stop_azas_all.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", ) PANEL_PROTECTED_PATTERNS = ( @@ -315,6 +1183,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: @@ -326,6 +1204,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 "" @@ -340,17 +1228,261 @@ 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", + "FASTDDS_BUILTIN_TRANSPORTS", + "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) @@ -370,17 +1502,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] = [] @@ -388,19 +1585,19 @@ 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 - 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) @@ -409,19 +1606,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 @@ -438,17 +1623,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) @@ -458,19 +1641,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 @@ -489,15 +1660,118 @@ 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 + + +def cleanup_camera_stack(*, grace_sec: float = 2.0) -> list[str]: + """Best-effort cleanup of stale RealSense drivers before restart.""" + events: list[str] = [] + old = processes.pop("start_camera", None) + if old is not None: + events.extend(terminate_process_tree(old, label="stored start_camera", grace_sec=grace_sec)) + events.extend(cleanup_matching_processes(CAMERA_STACK_PATTERNS, label="camera cleanup", grace_sec=grace_sec)) return events +def cleanup_side_grip_stack(*, grace_sec: float = 2.0) -> list[str]: + """Best-effort cleanup of stale one-shot side-grip processes before retry.""" + events: 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. + + 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. + + These commands are normally launched as blocking panel steps. If a timeout, + browser refresh, or operator interrupt leaves a child process alive, the next + panel run can observe old services/actions/nodes and behave inconsistently. + The explicit cleanup button therefore treats these as robot-stack residue. + """ + return cleanup_matching_processes( + RUN_STEP_STACK_PATTERNS, + label="run-step cleanup", + grace_sec=grace_sec, + ) + + +def stop_ros2_daemon() -> list[str]: + """Stop the ROS 2 CLI daemon so cleanup starts the next run from a fresh graph cache.""" + env = os.environ.copy() + env["ROS_DOMAIN_ID"] = str(env.get("AZAS_PANEL_ROS_DOMAIN_ID") or env.get("ROS_DOMAIN_ID") or DEFAULT_ROS_DOMAIN_ID) + env["ROS_LOCALHOST_ONLY"] = str(env.get("ROS_LOCALHOST_ONLY") or "0") + try: + completed = subprocess.run( + ["bash", "-lc", f"{ROS_SETUP} && ros2 daemon stop"], + cwd=str(ROOT), + env=env, + text=True, + capture_output=True, + timeout=8.0, + ) + except subprocess.TimeoutExpired: + return ["ros2 daemon stop: timed out"] + except Exception as exc: # pragma: no cover - operator diagnostics only. + return [f"ros2 daemon stop: failed: {exc}"] + + output = " ".join(part.strip() for part in (completed.stdout, completed.stderr) if part.strip()) + if completed.returncode == 0: + return [f"ros2 daemon stop: ok{': ' + output if output else ''}"] + return [f"ros2 daemon stop: rc={completed.returncode}{': ' + output if output else ''}"] + + def find_existing_doosan_launch() -> tuple[int | None, str]: """Return one existing Doosan launch PID/cmd if a bringup is already starting/running.""" if psutil is None: 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", ) @@ -562,10 +1836,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, @@ -603,7 +1878,67 @@ 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 motion_services_ready(service_prefix: str) -> tuple[bool, str]: +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] = {} +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: + return 0, output + match = re.search(r"Action servers:\s*(\d+)", output) + return (int(match.group(1)) if match else 0), output + + +def wait_for_action_server(action_name: str, *, timeout_sec: float = 15.0) -> tuple[bool, str]: + deadline = time.monotonic() + max(timeout_sec, 0.1) + last_output = "" + attempt = 0 + while time.monotonic() < deadline: + attempt += 1 + count, output = action_server_count(action_name) + last_output = output + if count > 0: + return True, f"action server became ready after {attempt} check(s): {action_name}\n{output}" + time.sleep(0.5) + 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, set[str]]: clean = service_prefix.strip("/") or "dsr01" required = { f"/{clean}/motion/move_line", @@ -614,8 +1949,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( @@ -629,9 +1964,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}" @@ -659,12 +1996,18 @@ def status_check_failure(output: str) -> str | None: f"[FAIL] robot_state={state_id}({robot_state_name(state_id)}) is not " "STATE_STANDBY(1). Robot is connected, but real motion is not ready." ) + action_match = re.search(r"Action servers:\s*(\d+)", output) + if action_match is not None and int(action_match.group(1)) < 1: + return ( + "[FAIL] MoveIt trajectory action server is not available: " + "/dsr01/dsr_moveit_controller/follow_joint_trajectory" + ) return None def run_output_failure(step: Step, output: str) -> str | None: - if step.key == "shake_closed_cup": - failure_markers = ( + if step.key in {"shake_closed_cup", "side_grip"}: + failure_markers = [ "]: FAILED", " returned success=false", " Ikin returned success=false", @@ -673,9 +2016,21 @@ def run_output_failure(step: Step, output: str) -> str | None: "MoveIt state validity is invalid", "Hardware gates are incomplete", "enable_hardware was requested but hardware gates are incomplete", - ) + ] + if step.key == "side_grip": + failure_markers.extend( + [ + "Action client not connected to action server", + "Failed to send trajectory", + "Completed trajectory execution with status ABORTED", + "MoveIt execution did not reach the requested pose", + "High camera home move failed", + "No valid depth around cup bbox", + "Exiting after one auto-pick attempt (success=False)", + ] + ) if any(marker in output for marker in failure_markers): - return "[FAIL] shake node reported FAILED/success=false even though ros2 launch exited cleanly." + return f"[FAIL] {step.key} reported an internal failure even though ros2 launch exited cleanly." return None @@ -689,8 +2044,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_"): @@ -702,11 +2055,29 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/motion/move_joint", f"/{clean}/motion/check_motion", f"/{clean}/system/get_robot_state", + ] + 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"}: + 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", ] @@ -720,32 +2091,33 @@ 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.startswith("press_dispenser_"): + if step.key == "run_color_recipe_sequence": return [ "/jarvis/rg2/set_width", - f"/{clean}/motion/move_joint", 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}/aux_control/get_current_posx", - f"/{clean}/tcp/set_current_tcp", f"/{clean}/tcp/get_current_tcp", + f"/{clean}/aux_control/get_current_posj", + f"/{clean}/aux_control/get_current_posx", ] - if step.key.startswith("pick_from_dispenser_"): + if step.key.startswith("press_dispenser_"): 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}/aux_control/get_current_posj", f"/{clean}/aux_control/get_current_posx", + f"/{clean}/tcp/set_current_tcp", + f"/{clean}/tcp/get_current_tcp", ] - if step.key == "run_dispenser_recipe_sequence": + if step.key.startswith("pick_from_dispenser_"): return [ "/jarvis/rg2/set_width", f"/{clean}/motion/move_joint", @@ -755,7 +2127,7 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: 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_posj", f"/{clean}/aux_control/get_current_posx", ] if step.key == "place_cup_holder": @@ -769,13 +2141,27 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: ] if step.key == "shake_closed_cup": return [ + "/jarvis/rg2/set_width", + f"/{clean}/motion/move_line", f"/{clean}/motion/move_joint", f"/{clean}/motion/move_wait", + f"/{clean}/motion/ikin", f"/{clean}/motion/check_motion", + f"/{clean}/aux_control/get_current_posx", f"/{clean}/aux_control/get_current_posj", 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 [] @@ -792,13 +2178,14 @@ 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 == "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", "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 in {"gripper_open", "gripper_soft_grasp"}: + if step.key == "gripper_soft_grasp": return 12.0 return 8.0 @@ -839,6 +2226,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 @@ -848,6 +2242,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 ( @@ -871,13 +2266,14 @@ 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 collision objects are visible.""" deadline = time.monotonic() + max(timeout_sec, 0.1) - last_output = "" + last_collision_output = "" + saw_collision = 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, @@ -887,13 +2283,21 @@ 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 + + if saw_collision: + return ( + True, + "collision object sample observed on /collision_object\n" + + last_collision_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})\n" + f"--- last /collision_object ---\n{last_collision_output}" ) @@ -943,6 +2347,190 @@ 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], + 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], @@ -974,6 +2562,288 @@ def wait_for_cup_detection_sample( f"--- last output ---\n{last_output}" ) + +def camera_snapshot_jpeg() -> tuple[bool, bytes, str]: + """Capture one RealSense color frame and return it as JPEG bytes.""" + script = r""" +import sys +import time + +import cv2 +import numpy as np +import rclpy +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image + +frame = None + +def to_bgr(msg): + 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 image encoding: {msg.encoding}") + +def callback(msg): + global frame + if frame is None: + frame = to_bgr(msg) + +rclpy.init() +node = rclpy.create_node("azas_panel_camera_snapshot") +node.create_subscription(Image, "/camera/camera/color/image_raw", callback, qos_profile_sensor_data) +deadline = time.time() + 3.0 +try: + while rclpy.ok() and frame is None and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if frame is None: + raise RuntimeError("no /camera/camera/color/image_raw frame within 3s") + ok, encoded = cv2.imencode(".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), 80]) + if not ok: + raise RuntimeError("cv2.imencode failed") + sys.stdout.buffer.write(encoded.tobytes()) +finally: + node.destroy_node() + rclpy.shutdown() +""" + env = shell_env({}) + proc = None + try: + proc = subprocess.Popen( + ["bash", "-lc", f"{ROS_SETUP} && python3 -c {shlex.quote(script)}"], + cwd=str(ROOT), + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + start_new_session=True, + ) + stdout, stderr = proc.communicate(timeout=6.0) + except subprocess.TimeoutExpired: + if proc is not None: + try: + os.killpg(proc.pid, signal.SIGTERM) + proc.wait(timeout=1.0) + except Exception: + try: + os.killpg(proc.pid, signal.SIGKILL) + except Exception: + pass + return False, b"", "camera snapshot timed out" + except Exception as exc: # pragma: no cover - operator diagnostics only. + return False, b"", f"camera snapshot failed: {exc}" + if proc.returncode != 0 or not stdout: + return False, b"", stderr.decode("utf-8", errors="replace")[-2000:] + return True, stdout, "" + + +def side_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, str]: + """Validate PR #20 manual side-grip prerequisites before starting MoveItPy. + + The PR #20 path opens its YOLO preview only after model/calibration loading + and MoveItPy initialization. If prerequisites are missing, the operator can + otherwise see "camera window does not open" while the real failure happened + earlier in startup. + """ + checks: list[str] = [] + ok = True + + model_path = PR20_YOLO_MODEL_PATH + if model_path.exists(): + checks.append(f"[OK] YOLO model: {model_path}") + else: + ok = False + checks.append(f"[FAIL] YOLO model missing: {model_path}") + + calibration_candidates = [ + ROOT / "install" / "dsr_practice" / "share" / "dsr_practice" / "config" / "T_gripper2camera.npy", + ROOT / "src" / "dsr_practice" / "config" / "T_gripper2camera.npy", + ROOT / "install" / "azas_perception" / "share" / "azas_perception" / "config" / "T_gripper2camera.npy", + ] + calibration_path = next((path for path in calibration_candidates if path.exists()), None) + if calibration_path is not None: + checks.append(f"[OK] hand-eye calibration: {calibration_path}") + else: + ok = False + checks.append( + "[FAIL] hand-eye calibration missing: " + + ", ".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 없이 켜져 있을 수 있음 → 자동 재시작 + 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 " + "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) + 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 + + 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: + 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}") + + 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: + 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) + + def ensure_gripper_services(step: Step, payload: dict[str, Any], service_prefix: str) -> tuple[bool, str]: required = gripper_services_for_step(step, service_prefix) if not required: @@ -1028,11 +2898,20 @@ 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_") - or step.key == "run_dispenser_recipe_sequence" + or step.key == "run_color_recipe_sequence" or step.key == "place_cup_holder" ) @@ -1103,6 +2982,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: @@ -1187,21 +3133,83 @@ 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 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", + "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_") ) def with_collision_scene_prereq(selected: list[str]) -> list[str]: - if not any(requires_collision_scene_step(key) for key in selected): + 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]: + """Run dispenser recipe inside manual OpenCV tmux step after success. + + The manual PR #20 / cup-uprighting steps are long-running GUI commands. + If the panel loop keeps `run_color_recipe_sequence` as a separate next + step, it starts immediately after the tmux window is opened, before the + operator presses `p`. Instead, mark the manual command for shell-level + chaining and remove the separate recipe step from the server loop. + """ + manual_keys = {"side_grip", "cup_uprighting"} + if not any(key in selected for key in manual_keys): return selected - return ["start_collision_scene"] + [key for key in selected if key != "start_collision_scene"] + payload["_auto_recipe_after_manual_logic"] = True + return [key for key in selected if key != "run_color_recipe_sequence"] + def run_timeout_for_step(step: Step) -> float: - if step.key == "run_dispenser_recipe_sequence": + if step.key == "side_grip": + return 300.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": + 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 @@ -1209,6 +3217,14 @@ def run_timeout_for_step(step: Step) -> float: def shell_env(payload: dict[str, Any]) -> dict[str, str]: env = os.environ.copy() + env["ROS_DOMAIN_ID"] = str( + payload.get("ros_domain_id") + or env.get("AZAS_PANEL_ROS_DOMAIN_ID") + or env.get("ROS_DOMAIN_ID") + or DEFAULT_ROS_DOMAIN_ID + ) + env["ROS_LOCALHOST_ONLY"] = str(env.get("ROS_LOCALHOST_ONLY") or "0") + env["FASTDDS_BUILTIN_TRANSPORTS"] = str(env.get("FASTDDS_BUILTIN_TRANSPORTS") or "UDPv4") env["ROBOT_HOST"] = str(payload.get("robot_host") or env.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST) env["ROBOT_NAME"] = str(payload.get("robot_name") or env.get("ROBOT_NAME") or "dsr01") env["SERVICE_PREFIX"] = str(payload.get("service_prefix") or env.get("SERVICE_PREFIX") or "dsr01") @@ -1217,24 +3233,54 @@ 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") or env.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") - or "0.0" + 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") + 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 + # cup deeper during place_cup_holder. + env["CUP_HOLDER_PICK_Z_OFFSET_M"] = str( + payload.get("cup_holder_pick_z_offset_m") + or env.get("CUP_HOLDER_PICK_Z_OFFSET_M") + or "-0.020" ) 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_NO_MOTION_CONFIRM"] = "CONNECT_DOOSAN_NO_MOTION" + 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 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: @@ -1245,36 +3291,103 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"-p dispenser_id:={shlex.quote(dispenser_id)} " "-p publish_once:=true" ) - - if step.key == "connect_robot": + + if step.key == "connect_robot": + return tmux_stack_start_command(payload) + if step.key == "start_tmux_stack": + return tmux_stack_start_command(payload) + if step.key == "status_check": + clean = service_prefix.strip("/") or "dsr01" + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "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"(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 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 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"(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 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 ---' && " + "(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') && " + "(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 '--- 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": + return color_recipe_sequence_command(payload) + 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 == "stop_azas_all": + # No ROS_SETUP: the cleanup script must not depend on (or re-spawn) the + # ROS daemon it is about to kill. + return f"cd {ROOT} && bash tools/run/stop_azas_all.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 "dsr01") - rt_host = str( - payload.get("rt_host") - or os.environ.get("RT_HOST") - or infer_rt_host(robot_host) - or "" + 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} && 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 " - f"{step.command}" + f"cd {ROOT} && {ROS_SETUP} && " + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " + "tools/run/check_one_click_cocktail_result.sh" ) - if step.key == "status_check": - clean = service_prefix.strip("/") or "dsr01" + 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} && " - "echo '--- nodes ---' && ros2 node list && " - "echo '--- required motion service types ---' && " - f"ros2 service type /{clean}/motion/move_line && " - f"ros2 service type /{clean}/motion/move_joint && " - "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 && " - "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" + "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 = { @@ -1288,7 +3401,25 @@ 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": + 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 --motion-timeout-sec 120 " + "--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 --motion-timeout-sec 120 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "home_robot": @@ -1296,71 +3427,158 @@ 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} " + "--motion-timeout-sec 120 --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} " + 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 --motion-timeout-sec 120 " "--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" 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": 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:=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 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)} & " + f"python3 {shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'azas_bringup' / 'collision_scene_rviz_publisher.py'))} " + "--ros-args " + f"-p safety_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'safety.yaml'))} " + f"-p dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " + f"-p calibration_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'calibration.yaml'))} " + "-p publish_workspace_ceiling:=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" + ")" ) 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" + "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" ) - if step.key == "detect_cup_lid": - return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_bringup yolo_perception.launch.py" - if step.key == "voice_input": - return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_voice azas_voice.launch.py" - if step.key == "side_grip": - raw_dispenser_id = str( - payload.get("selected_dispenser_id") - or os.environ.get("SELECTED_DISPENSER_ID") - or "2" - ).strip() - selected_dispenser_id = ( - raw_dispenser_id - if raw_dispenser_id.startswith("dispenser_") - else f"dispenser_{raw_dispenser_id}" + if step.key == "start_camera_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 /camera/camera/color/image_raw" ) - model_path = str( - payload.get("model_path") - or os.environ.get("MODEL_PATH") - or "/home/ssu/Downloads/로봇 데이터/best.pt" + 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": return ( f"cd {ROOT} && {ROS_SETUP} && " + "ros2 launch azas_bringup lid_sticker_grip_planning.launch.py" + ) + if step.key == "lid_grip_close": + direct_script = ROOT / "tools" / "run" / "run_kang_lid_grip_close_direct.sh" + manual_cmd = ( + f"cd {ROOT} && " + 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:-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))}" + ) + 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 = ( + f"cd {ROOT} && " + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " - "PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} " - "ros2 launch azas_bringup yolo_cup_pick_node_legacy.launch.py " - f"model_path:={shlex.quote(model_path)} " - "auto_pick:=true " - "exit_after_pick_attempt:=true " - "grasp_mode:=side " - f"moveit_namespace:=/{shlex.quote(service_prefix.strip('/') or 'dsr01')} " - f"selected_dispenser_id:={shlex.quote(selected_dispenser_id)} " - "use_measured_front_hold_pose:=true " - "publish_dispenser_collision_objects:=true" - ) - if step.key == "gripper_open": + f"MODEL_PATH={shlex.quote(str(CUP_UPRIGHTING_YOLO_MODEL_PATH))} " + "EXIT_AFTER_PICK=true " + f"bash {shlex.quote(str(direct_script))}" + ) + if payload.get("_auto_recipe_after_manual_logic"): + 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} && " - "tools/run/rg2_full_open_verify.sh" + "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" + ) + 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))}" ) + 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": return ( f"cd {ROOT} && {ROS_SETUP} && " @@ -1394,7 +3612,10 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"python3 tools/run/move_to_measured_dispenser_front_hold.py " f"--service-prefix {service_prefix} --dispenser-id {shlex.quote(dispenser_id)} " "--timeout-sec 180 --verify-target --verify-timeout-sec 70 " - "--target-tolerance-mm 15 --compensate-current-tcp --verify-link6-target --no-moveit-planning-guard " + "--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 " ) # 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 @@ -1413,7 +3634,12 @@ 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") @@ -1421,15 +3647,28 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe ).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_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:=true " - f"-p target_dispenser:={shlex.quote(target)} " - "-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 " @@ -1439,8 +3678,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 " @@ -1466,45 +3709,49 @@ 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-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)}" ) - 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") or os.environ.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") - or "0.0" + 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") + 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} " "--config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml " - "--approach-velocity 15.0 --approach-acceleration 20.0 " + "--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 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)} " - "--place-velocity 6.0 --place-acceleration 10.0 " - "--retreat-velocity 12.0 --retreat-acceleration 16.0 " + 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 " "--z-max 0.28 " "--execute --confirm ENABLE_CUP_HOLDER_PLACE" @@ -1512,18 +3759,43 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f" && {tumbler_scene_once('add_holder', object_id='tumbler_in_holder')}" ) if step.key == "shake_closed_cup": + pick_z_offset_m = str( + payload.get("cup_holder_pick_z_offset_m") + or os.environ.get("CUP_HOLDER_PICK_Z_OFFSET_M") + or "-0.020" + ).strip() + holder_pick = ( + "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 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 25.0 " + "--post-grasp-settle-sec 0.8 " + "--z-max 0.28 " + "--execute --confirm ENABLE_CUP_HOLDER_PICK" + ) return ( f"cd {ROOT} && " - "echo '[Azas] SHAKE START 설명: 컵홀더에 놓인 닫힌 컵을 side grip으로 다시 잡은 뒤 흔드는 단계입니다.' && " - "echo '[Azas] 순서: 컵홀더 place 완료 확인 -> RG2가 컵 몸통/홀더 쪽을 안정적으로 잡은 상태 확인 -> 들어 올림/관절 쉐이킹 실행.' && " - "echo '[Azas] 주의: 이 버튼은 컵 좌표를 새로 만들지 않으며, 컵이 이미 닫혀 있고 그리퍼에 잡힌 상태를 전제로 합니다.' && " - f"SERVICE_PREFIX={service_prefix} GRASPED_CUP_TEST_MODE=true " - "REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=4 " + f"{ROS_SETUP} && " + "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 측정값만 사용합니다.' && " + f"{holder_pick}" + f" && {tumbler_scene_once('remove_world', object_id='tumbler_in_holder')}" + f" && {tumbler_scene_once('attach', object_id='carried_tumbler')}" + " && " + f"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=25.0 " - "JOINT_SHAKE_J5_AMPLITUDE_DEG=30.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=37.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 " @@ -1531,14 +3803,46 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "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=180.0 " - "SHAKE_JOINT_ACCELERATION=260.0 SHAKE_JOINT_TIME=0.0 " - "JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=225.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 " + 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( + 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 "" @@ -1547,7 +3851,72 @@ 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.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 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 ( + f"[Azas] field-verified tmux mode: {label}은 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) if not gripper_ready: @@ -1585,107 +3954,110 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: f"{ready_output}" ), } + 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=3.0) + if not action_ready: + # 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 ): return {"key": step.key, "status": "blocked", "output": "ROBOT_HOST가 필요합니다."} - cmd = command_for(step, payload) env = shell_env(payload) + preflight_output = "" + if step.key == "side_grip": + cleanup_events = cleanup_side_grip_stack(grace_sec=3.0) + # Let DDS forget stale PR #20 picker/relay nodes before MoveItPy starts. + time.sleep(1.0) + service_prefix = str(payload.get("service_prefix") or "dsr01") + preflight_ok, preflight_details = side_grip_preflight(env, service_prefix) + preflight_output = "\n".join( + cleanup_events + + [ + "--- PR #20 side_grip preflight ---", + preflight_details, + ] + ).strip() + if not preflight_ok: + return { + "key": step.key, + "status": "blocked", + "output": ( + "PR #20 side grip 실행 전 조건이 충족되지 않아 시작하지 않았습니다.\n" + 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": restart_output = "" - if step.key == "connect_robot": - ready, ready_output = motion_services_ready(env["SERVICE_PREFIX"]) - if ready: - 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", - "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( - env["SERVICE_PREFIX"], - timeout_sec=20.0, - proc=old, + if step.key in {"connect_robot", "start_tmux_stack"}: + 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 ready: - robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) - if robot_ready: - return { - "key": step.key, - "status": "running", - "output": ( - "기존 로봇 연결 프로세스가 계속 실행 중이고 motion 서비스가 준비됐습니다.\n" - f"{waited_output}\n{robot_ready_output}" - ), - "pid": old.pid, - } - return { - "key": step.key, - "status": "blocked", - "output": ( - "기존 로봇 연결 프로세스가 motion 서비스를 띄웠지만 로봇이 " - "STATE_STANDBY(1)가 아닙니다.\n" - f"{waited_output}\n{robot_ready_output}" - ), - "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, - } - 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 part + ) elif step.key == "connect_gripper": cleanup_events = cleanup_rg2_stack() # DDS may keep stale service names briefly after a killed RG2 wrapper. time.sleep(1.0) restart_output = "\n".join(cleanup_events) + elif step.key == "start_camera": + 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": + # 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: @@ -1695,6 +4067,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") @@ -1710,49 +4084,35 @@ 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 == "connect_robot": - ready, waited_output = wait_for_motion_services_ready( - env["SERVICE_PREFIX"], - timeout_sec=35.0, - proc=proc, - ) - if ready: - robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) - output = f"{cmd}\n--- log ---\n{log_path}\n--- readiness ---\n{waited_output}\n{robot_ready_output}" - if restart_output: - output = f"{restart_output}\n--- start command ---\n{output}" - if robot_ready: - return { - "key": step.key, - "status": "started", - "pid": proc.pid, - "output": output, - } - return { - "key": step.key, - "status": "blocked", - "pid": proc.pid, - "output": ( - "로봇 연결 프로세스는 시작됐고 motion 서비스도 보이지만 " - "로봇이 STATE_STANDBY(1)가 아닙니다.\n" - + output - ), - } - if proc.poll() is None: + if step.key in {"start_tmux_stack", "connect_robot"}: + try: + proc.wait(timeout=90.0) + except subprocess.TimeoutExpired: output = ( - f"{cmd}\n--- log ---\n{log_path}\n--- readiness ---\n{waited_output}\n" - "아직 시작 중입니다. 몇 초 뒤 '연결 확인'만 다시 눌러주세요." + f"{cmd}\n--- log ---\n{log_path}\n" + "tmux 통합 재연결 명령이 90초 안에 종료되지 않았습니다.\n" + + tail_file(log_path, max_chars=8000) ) if restart_output: output = f"{restart_output}\n--- start command ---\n{output}" - return { - "key": step.key, - "status": "starting", - "pid": proc.pid, - "output": output, - } - else: - time.sleep(2.0) + 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 restart_output: + output = f"{restart_output}\n--- start command ---\n{output}" + 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 오류 때문에 이 단계에서 후속 조회로 차단하지 않습니다." + ), + } + + time.sleep(2.0) if proc.poll() is not None: output = tail_file(log_path) if restart_output: @@ -1766,36 +4126,55 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: output = f"{restart_output}\n--- start command ---\n{output}" if ready: output += ( - "\n[Azas] RG2 ROS services are ready. Note: jarvis RG2 wrapper has no physical " + "\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: output = f"{restart_output}\n--- start command ---\n{cmd}" output += f"\n--- log ---\n{log_path}" + if step.key == "side_grip": + output += ( + "\n[Azas] PR #20 manual side_grip 노드를 백그라운드로 시작했습니다. " + "YOLO/OpenCV 창에서 컵을 확인한 뒤 p 키를 누르면 잡기 동작이 실행되고, " + "Esc/q로 종료합니다. 패널은 수동 입력 대기 때문에 더 이상 3분씩 블로킹하지 않습니다." + f"\n--- log tail ---\n{tail_file(log_path, max_chars=4000)}" + ) return { "key": step.key, "status": "started", @@ -1804,19 +4183,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 - if completed.returncode == 0: + 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: failure = run_output_failure(step, output) if failure is not None: output = f"{output}\n{failure}\n" @@ -1836,6 +4243,33 @@ 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")) + 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]}") + 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) @@ -1849,20 +4283,50 @@ 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: - return {"key": step.key, "status": "timeout", "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": "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]] = [] for key, proc in list(processes.items()): 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} @@ -1870,6 +4334,11 @@ def cleanup_all_processes() -> dict[str, Any]: """Explicit operator cleanup button: stop tracked jobs plus stale robot/panel helpers.""" stopped = stop_all() 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_rg2_stack(grace_sec=3.0)) events.extend(cleanup_doosan_stack(grace_sec=3.0)) events.extend( cleanup_matching_processes( @@ -1878,17 +4347,24 @@ def cleanup_all_processes() -> dict[str, Any]: grace_sec=3.0, ) ) + events.extend(stop_ros2_daemon()) + process_logs.clear() return {"stopped": stopped.get("stopped", []), "cleanup": events} 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 @@ -1901,6 +4377,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"), @@ -1910,14 +4387,63 @@ 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" + ), + "cup_holder_place_final_z_offset_m": os.environ.get( + "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 = [] 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 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 + 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) + message = (error or "camera snapshot unavailable").encode("utf-8", errors="replace") + 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) + return + self.send_response(200) + self.send_header("Content-Type", "image/jpeg") + self.send_header("Cache-Control", "no-store") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + return self.send_json({"error": "not found"}, 404) def do_POST(self) -> None: @@ -1925,14 +4451,78 @@ 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 []]) - 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({"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)) + selected = configure_manual_recipe_chain(selected, payload) + 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 + 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") + 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) + _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 "") + 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()) @@ -1949,7 +4539,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_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh new file mode 100755 index 0000000..322ec1b --- /dev/null +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -0,0 +1,140 @@ +#!/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:-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 +source "${ROOT}/install/dsr_practice/share/dsr_practice/package.bash" +set -u + +export DISPLAY XAUTHORITY ROS_DOMAIN_ID ROS_LOCALHOST_ONLY FASTDDS_BUILTIN_TRANSPORTS +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. 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} FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS}" +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 \ + --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 & + +# 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 + +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" \ + --ros-args -r __node:=azas_joint_state_relay \ + -p input_topic:=/"${SERVICE_PREFIX}"/joint_states \ + -p output_topic:=/joint_states + ) & +fi + +side_grip_success_log="$(mktemp /tmp/azas_changhyun_side_grip.XXXXXX.log)" +set +e +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}" \ + 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_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 \ + 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.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 \ + 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 \ + 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_cocktail_collision_rviz_preview.sh b/tools/run/run_cocktail_collision_rviz_preview.sh new file mode 100755 index 0000000..1ff245e --- /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:-0}" \ + 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_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py new file mode 100644 index 0000000..37c5c6c --- /dev/null +++ b/tools/run/run_color_recipe_sequence.py @@ -0,0 +1,682 @@ +#!/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 # 직접 지정 + python3 tools/run/run_color_recipe_sequence.py --dispenser-ids 1x1,2x2,3x1 +""" +from __future__ import annotations + +import argparse +import json +import os +import re +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 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(*, 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}", file=sys.stderr) + return {} + data = json.loads(COLOR_MAP_PATH.read_text(encoding="utf-8")) + mapped = normalize_color_map(data) + if not mapped or all(v == "unknown" for v in mapped.values()): + print("[run_color_recipe] 색상 맵이 비어 있거나 전부 unknown", file=sys.stderr) + return {} + return mapped + + +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]]: + """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 item: + color, count_raw = item.split(":", 1) + elif "x" in item: + color, count_raw = item.split("x", 1) + else: + match = 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 + + +def parse_recipe_data(recipe: object) -> list[tuple[str, int]]: + """Return color pump counts from supported recipe JSON shapes. + + Supported examples: + {"colors": ["red", "green"], "pumps": {"red": 3, "green": 3}} + {"pumps": {"red": 3, "green": 3}} + {"red": 3, "green": 3} + [{"color": "red", "count": 3}, {"color": "green", "pumps": 3}] + """ + color_pumps: list[tuple[str, int]] = [] + + def add(color: object, count: object = 1) -> None: + color_name = str(color).lower().strip() + if not color_name: + return + try: + pump_count = int(count) + except (TypeError, ValueError): + raise ValueError(f"invalid pump count for color {color_name}: {count!r}") + if pump_count < 1: + raise ValueError(f"pump count must be >= 1 for color {color_name}") + color_pumps.append((color_name, pump_count)) + + if isinstance(recipe, list): + for item in recipe: + if isinstance(item, dict): + color = item.get("color") or item.get("name") + count = item.get("pumps", item.get("count", item.get("presses", 1))) + if color: + add(color, count) + else: + add(item, 1) + return color_pumps + + if not isinstance(recipe, dict): + raise ValueError("recipe JSON must be an object or list") + + colors = recipe.get("colors") + pumps = None + for key in ("pumps", "presses", "counts"): + if key in recipe: + pumps = recipe.get(key) + break + + if isinstance(colors, list): + if not isinstance(pumps, dict): + pumps = {} + for color in colors: + key = str(color).lower().strip() + add(key, pumps.get(key, pumps.get(str(color), 1))) + return color_pumps + + if isinstance(pumps, dict): + for color, count in pumps.items(): + add(color, count) + return color_pumps + + # Compact operator JSON: {"red": 3, "green": 3} + metadata_keys = {"source", "note", "notes", "created_at", "updated_at"} + for color, count in recipe.items(): + if str(color).lower().strip() in metadata_keys: + continue + if isinstance(count, (int, float, str)): + add(color, count) + + return color_pumps + + +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', '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( + "--service-prefix", + default=os.environ.get("SERVICE_PREFIX", ""), + help="Doosan direct service namespace. 현재 스택이 /motion/* 루트 서비스를 쓰면 빈 값", + ) + parser.add_argument( + "--confirm", + nargs="?", + const=CONFIRM_PHRASE, + default="", + help=( + f"실행 확인. 값 없이 --confirm만 써도 되고, 기존 습관대로 확인 문구를 붙여도 됩니다. " + f"내부 measured sequence에는 {CONFIRM_PHRASE}를 전달합니다." + ), + ) + 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") + parser.add_argument("--move-prehold-acceleration", default="22.0") + parser.add_argument("--pick-approach-velocity", default="80.0") + parser.add_argument("--pick-approach-acceleration", default="14.0") + parser.add_argument("--pick-lift-velocity", default="80.0") + parser.add_argument("--pick-lift-acceleration", default="25.0") + parser.add_argument("--regrasp-approach-velocity", default="80.0") + parser.add_argument("--regrasp-approach-acceleration", default="18.0") + parser.add_argument( + "--regrasp-reset-before-cup", + action=argparse.BooleanOptionalAction, + default=True, + help="프레스 후 컵 재접근 전에 HOME joint waypoint를 경유", + ) + parser.add_argument("--regrasp-reset-joints-deg", default="0,0,90,0,90,180") + 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="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( + "--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, + default=True, + 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="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", + default="0.0", + help="--press-depth-m에 추가할 Z-only 프레스 하강량. 기본 0.", + ) + parser.add_argument( + "--press-lock-contact-joints", + default="", + help=( + "measured sequence로 전달할 contact 조인트 잠금 축. 기본 빈 값: " + "측정된 PRESS_CONTACT joint를 그대로 사용합니다." + ), + ) + parser.add_argument("--press-pre-lift-m", default="0.080") + parser.add_argument("--press-transit-height-m", default="0.080") + # Base frame in the measured dispenser setup: + # +X points from the robot toward the dispenser body, so backing away + # from the dispenser toward the robot is negative X. + # Y separates dispenser slots left/right. Do not use Y as a safety + # retreat; on dispenser 4 it pushes the cup farther to the robot-view + # right side and can move outside the measured dispenser footprint. + parser.add_argument("--press-pre-lift-retreat-x-m", default="-0.050") + 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.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", + 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, + default=False, + help="저장된 cup_common_pre_joints_deg를 사용. 기본 false: cup_place 기준 X/Z offset pre 생성", + ) + 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.350") + parser.add_argument( + "--start-safe-lift-z-m", + default="0.15", + help="시퀀스 시작 시 현재 TCP pose에서 Z-only로 먼저 올라갈 최소 절대 TCP Z (m)", + ) + parser.add_argument( + "--min-allowed-tcp-z-m", + default="0.02", + help="모든 cartesian target pose의 최소 허용 TCP Z (m). 미달 target은 로봇 명령 전송 전에 차단", + ) + parser.add_argument( + "--force-start-safe-lift", + action=argparse.BooleanOptionalAction, + default=True, + help="시작 시 항상 현재 위치에서 safe Z로 lift한 뒤 다음 waypoint로 이동 (기본 켜짐)", + ) + parser.add_argument( + "--skip-release-pre", + action=argparse.BooleanOptionalAction, + default=False, + help="cup_place 기준 X/Z offset release-pre 대신 자세 무관 안전 release 구조 사용", + ) + parser.add_argument("--release-approach-lift-m", default="0.100", + help="release final 위에 생성할 release_above pose의 Z lift (m)") + parser.add_argument("--release-start-safe-lift-m", default="0.120", + help="release 시작 시 현재 TCP에서 Z-only로 올릴 상대 높이 (m)") + parser.add_argument("--release-min-transit-z-m", default="0.300", + help="release XY-transit pose의 최소 절대 TCP Z (m)") + parser.add_argument("--post-release-safe-lift-m", default="0.100", + help="gripper open 후 release final에서 수직 상승할 높이 (m)") + parser.add_argument("--release-staging-x-m", default="", + help="release staging pose 절대 X (m). 비우면 release final X 사용") + parser.add_argument("--release-staging-y-m", default="", + help="release staging pose 절대 Y (m). 비우면 release final Y 사용") + parser.add_argument("--release-staging-z-m", default="", + help="release staging pose 절대 Z (m). 비우면 transit 높이 사용") + parser.add_argument( + "--use-release-staging", + action=argparse.BooleanOptionalAction, + default=True, + help="release_above 진입 전에 staging pose를 경유 (기본 켜짐)", + ) + 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("--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="25.0") + parser.add_argument( + "--allow-tcp-set-failure", + action="store_true", + help="Doosan TCP 설정 서비스가 success=false를 반환해도 현재 TCP로 measured sequence를 계속 실행", + ) + 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( + "--place-cup-holder-after-sequence", + action=argparse.BooleanOptionalAction, + default=True, + 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-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", + 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") + parser.add_argument("--cup-holder-place-velocity", default="80.0") + parser.add_argument("--cup-holder-place-acceleration", default="10.0") + parser.add_argument("--cup-holder-retreat-velocity", default="80.0") + parser.add_argument("--cup-holder-retreat-acceleration", default="16.0") + parser.add_argument("--cup-holder-timeout-sec", default="90.0") + parser.add_argument("--cup-holder-target-tolerance-mm", default="12.0") + 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("--safe-lift-target-tolerance-mm", default="30.0") + parser.add_argument("--post-press-safe-lift-target-tolerance-mm", default="60.0") + 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) + 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 = [ + "--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", 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), + "--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), + "--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), + "--post-press-safe-lift-z-m", str(args.post_press_safe_lift_z_m), + "--start-safe-lift-z-m", str(args.start_safe_lift_z_m), + "--min-allowed-tcp-z-m", str(args.min_allowed_tcp_z_m), + "--release-approach-lift-m", str(args.release_approach_lift_m), + "--release-start-safe-lift-m", str(args.release_start_safe_lift_m), + "--release-min-transit-z-m", str(args.release_min_transit_z_m), + "--post-release-safe-lift-m", str(args.post_release_safe_lift_m), + "--regrasp-rear-entry-offset-x-m", str(args.regrasp_rear_entry_offset_x_m), + "--regrasp-rear-entry-offset-y-m", str(args.regrasp_rear_entry_offset_y_m), + "--final-regrasp-extra-x-offset-m", str(args.final_regrasp_extra_x_offset_m), + "--final-regrasp-extra-y-offset-m", str(args.final_regrasp_extra_y_offset_m), + "--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", 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", 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), + "--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-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), + "--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), + "--pose-read-retries", str(args.pose_read_retries), + "--pose-read-retry-sleep-sec", str(args.pose_read_retry_sleep_sec), + "--safe-lift-target-tolerance-mm", str(args.safe_lift_target_tolerance_mm), + "--post-press-safe-lift-target-tolerance-mm", str(args.post_press_safe_lift_target_tolerance_mm), + "--safe-lift-joint-fallback", + "--no-integrated-regrasp-fallback-subprocess", + ] + sequence_extra_args.append( + "--force-start-safe-lift" if args.force_start_safe_lift else "--no-force-start-safe-lift" + ) + 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" + ) + sequence_extra_args.append( + "--use-release-staging" if args.use_release_staging else "--no-use-release-staging" + ) + sequence_extra_args.append( + "--place-cup-holder-after-sequence" + if args.place_cup_holder_after_sequence + else "--no-place-cup-holder-after-sequence" + ) + for flag, value in ( + ("--release-staging-x-m", args.release_staging_x_m), + ("--release-staging-y-m", args.release_staging_y_m), + ("--release-staging-z-m", args.release_staging_z_m), + ): + if str(value).strip(): + sequence_extra_args += [flag, str(value).strip()] + 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" + ) + if args.allow_tcp_set_failure: + 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: + 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, + *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 + + 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: + 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"] + 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: + 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) + print("[run_color_recipe] listen_stt_recipe 스텝을 먼저 실행하세요.", file=sys.stderr) + return 1 + recipe = json.loads(RECIPE_PATH.read_text(encoding="utf-8")) + try: + color_pumps = parse_recipe_data(recipe) + except ValueError as exc: + print(f"[run_color_recipe] 레시피 JSON 파싱 실패: {exc}", file=sys.stderr) + return 1 + if not color_pumps: + print(f"[run_color_recipe] 레시피에 실행할 색상/펌프 수가 없습니다: {RECIPE_PATH}", file=sys.stderr) + return 1 + + print(f"[run_color_recipe] 레시피 색깔+펌프: {color_pumps}") + + # 색깔 → 디스펜서 ID 매핑 + sequence: list[str] = [] + mapped_steps: 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 + mapped_steps.append(f"{color}->{did}x{pumps}") + 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] 색상→디스펜서 상세: {', '.join(mapped_steps)}") + print(f"[run_color_recipe] 실행 순서: {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 __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/run_color_scan_stage.sh b/tools/run/run_color_scan_stage.sh new file mode 100755 index 0000000..3015c78 --- /dev/null +++ b/tools/run/run_color_scan_stage.sh @@ -0,0 +1,81 @@ +#!/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가 진행되므로, 이 단계는 레시피 전에 반드시 성공해야 한다. +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:-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:-} + +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:-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 + 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_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh new file mode 100755 index 0000000..9b16f9c --- /dev/null +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -0,0 +1,407 @@ +#!/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=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_STATE_RELAY_INPUT_TOPIC="${JOINT_STATE_RELAY_INPUT_TOPIC:-/${ROBOT_NAME}/joint_states}" +JOINT_STATES_TOPIC="${JOINT_STATES_TOPIC:-/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 +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:-0}" +WORKSPACE_COLLISION_ENABLED="${WORKSPACE_COLLISION_ENABLED:-1}" +FULL_COLLISION_SCENE_ENABLED="${FULL_COLLISION_SCENE_ENABLED:-1}" +FULL_COLLISION_SHOW_CEILING="${FULL_COLLISION_SHOW_CEILING:-0}" +SHOW_LINK6_GRIPPER="${SHOW_LINK6_GRIPPER:-1}" +START_JOINT_STATE_RELAY="${START_JOINT_STATE_RELAY:-auto}" +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 +SAFETY_CONFIG="${SAFETY_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/safety.yaml}" +if [[ ! -f "${SAFETY_CONFIG}" ]]; then + SAFETY_CONFIG="${ROOT_DIR}/src/azas_bringup/config/safety.yaml" +fi +CALIBRATION_CONFIG="${CALIBRATION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/calibration.yaml}" +if [[ ! -f "${CALIBRATION_CONFIG}" ]]; then + CALIBRATION_CONFIG="${ROOT_DIR}/src/azas_bringup/config/calibration.yaml" +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 + 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}" +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 + if [[ -n "${existing}" ]]; then + 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' | 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' | grep -v 'pgrep -af' >&2 || true + exit 1 +fi + +before_rviz="$(pgrep -x rviz2 || true)" + +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+=("$!") + + 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] Joint-state relay source: ${JOINT_STATE_RELAY_INPUT_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 + +if [[ "${START_JOINT_STATE_RELAY}" == "1" || "${START_JOINT_STATE_RELAY}" == "true" || "${START_JOINT_STATE_RELAY}" == "auto" ]]; then + if [[ "${JOINT_STATES_TOPIC}" == "/joint_states" && "${JOINT_STATE_RELAY_INPUT_TOPIC}" != "/joint_states" ]]; then + if ! pgrep -af "joint_state_relay.py.*input_topic:=${JOINT_STATE_RELAY_INPUT_TOPIC}.*output_topic:=/joint_states|joint_state_relay.py.*output_topic:=/joint_states.*input_topic:=${JOINT_STATE_RELAY_INPUT_TOPIC}" | grep -v 'pgrep -af' >/dev/null; then + python3 "${ROOT_DIR}/src/dsr_practice/dsr_practice/joint_state_relay.py" \ + --ros-args \ + -r __node:=azas_course_joint_state_relay \ + -p input_topic:="${JOINT_STATE_RELAY_INPUT_TOPIC}" \ + -p output_topic:="${JOINT_STATES_TOPIC}" \ + >"${LOG_DIR}/course_joint_state_relay.log" 2>&1 & + PIDS+=("$!") + echo "[Azas] Started joint-state relay: ${JOINT_STATE_RELAY_INPUT_TOPIC} -> ${JOINT_STATES_TOPIC}" + else + echo "[Azas] Reusing existing joint-state relay: ${JOINT_STATE_RELAY_INPUT_TOPIC} -> ${JOINT_STATES_TOPIC}" + fi + fi +fi + +joint_deadline=$((SECONDS + JOINT_WAIT_SEC)) +while (( SECONDS < joint_deadline )); do + 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 + 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_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: 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 [[ "${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 + 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 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} (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 + 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 [[ "${FULL_COLLISION_SCENE_ENABLED}" == "1" || "${FULL_COLLISION_SCENE_ENABLED}" == "true" ]]; then + if [[ "${FULL_COLLISION_SHOW_CEILING}" == "1" || "${FULL_COLLISION_SHOW_CEILING}" == "true" ]]; then + FULL_COLLISION_SHOW_CEILING_BOOL=true + else + FULL_COLLISION_SHOW_CEILING_BOOL=false + fi + if pgrep -af 'collision_scene_rviz_publisher.py|collision_scene_rviz_publisher' | grep -v 'pgrep -af' >/dev/null; then + echo "[Azas] Reusing existing full collision RViz publisher on /azas/collision_scene/markers." + else + python3 "${ROOT_DIR}/src/azas_bringup/azas_bringup/collision_scene_rviz_publisher.py" \ + --ros-args \ + -p safety_config_path:="${SAFETY_CONFIG}" \ + -p dispenser_collision_config_path:="${DISPENSER_COLLISION_CONFIG}" \ + -p calibration_path:="${CALIBRATION_CONFIG}" \ + -p publish_workspace_ceiling:="${FULL_COLLISION_SHOW_CEILING_BOOL}" \ + >"${LOG_DIR}/full_collision_scene_rviz_publisher.log" 2>&1 & + PIDS+=("$!") + echo "[Azas] FULL_COLLISION_SCENE_ENABLED=${FULL_COLLISION_SCENE_ENABLED}: publishing table/walls/dispenser/front-hold markers on /azas/collision_scene/markers." + fi +fi + +if [[ "${SHOW_LINK6_GRIPPER}" == "1" || "${SHOW_LINK6_GRIPPER}" == "true" ]]; then + ros2 launch azas_bringup rg2_link6_tcp.launch.py \ + 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 only; MoveIt uses the mesh-based RG2 URDF." +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 [[ "${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 + 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}" \ + 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.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|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_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" +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_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..6d64dd4 --- /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:-0.0.0.0}" +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_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_human_hand_detection.sh b/tools/run/run_human_hand_detection.sh new file mode 100755 index 0000000..302c234 --- /dev/null +++ b/tools/run/run_human_hand_detection.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# Perception-only human hand detection for the post-shake handover plan. +# Publishes /azas/human_hand_detection (PointStamped, camera optical frame), +# /azas/human_hand_detection/status (JSON), and an overlay image. +# This NEVER sends a robot motion command (gate: no_motion_hri_perception_only). +# Note: no `set -u`; ROS setup.bash references unset vars. +set -eo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" + +source /opt/ros/humble/setup.bash +if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" +fi + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +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_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh new file mode 100755 index 0000000..fb9698f --- /dev/null +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -0,0 +1,136 @@ +#!/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_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="${LID_ROS_DOMAIN_ID:-${ROS_DOMAIN_ID:-9}}" +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}" + +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 FASTDDS_BUILTIN_TRANSPORTS +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +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 + 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} 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 + 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 40 --acceleration 40 \ + --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 \ + --execute --confirm ENABLE_DIRECT_MOVEJ +fi + +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}" \ + 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 \ + 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}" \ + 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:="${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:=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 \ + lid_twist_rx:=73.901489 lid_twist_ry:=-178.542740 lid_twist_rz:=117.385612 \ + 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_mode:=j6_step_wiggle \ + lid_twist_preseat_periodic_descend_m:=0.02 lid_twist_preseat_step_m:=0.005 \ + 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 \ + 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 + launch_args+=(aruco_fallback_markers:="${ARUCO_FALLBACK_MARKERS}") +fi + +ros2 launch "${launch_args[@]}" 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 < 컵홀더 재픽업 -> 쉐이킹 -> 카메라 포즈 복귀. +# robot_pipeline_control_server.py chain_shake_after_lid_command()가 생성하는 패널 체인과 동일한 명령을 +# auto_cup_flow_router가 직접 실행할 수 있도록 스크립트로 고정한 것이다. + +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:-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 e047448..da6a9c5 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -1,17 +1,19 @@ #!/usr/bin/env python3 """Run an ordered measured-dispenser recipe loop. -For each dispenser ID this composes existing field primitives: - move/release cup at measured front-hold -> press dispenser -> re-grasp/lift cup. +For each dispenser ID this composes measured rule-based primitives: + move/release cup at DISP_PRE/DISP_PLACE -> measured PRESS_CONTACT + -> generated CONTACT_ENTRY_LIFT -> Z-only pump overdrive + -> re-grasp/lift cup at DISP_PLACE. -All cup/dispenser positions come from measured front_hold_poses and taught -press poses used by existing nodes. This runner does not ask for or generate -new robot coordinates. +All cup/dispenser positions come from measured calibration data. This runner +does not ask for or generate new robot coordinates. """ from __future__ import annotations import argparse +import json import math import shlex import subprocess @@ -24,11 +26,23 @@ 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, + GetCurrentTcp, + Ikin, + MoveJoint, + MoveLine, + MoveWait, + SetCurrentTcp, +) 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" @@ -42,6 +56,14 @@ "3": "yellow", "4": "blue", } +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} +INVALID_CUP_PLACE_STATUSES = INVALID_PRESS_CONTACT_STATUSES DR_BASE = 0 MOVE_MODE_ABSOLUTE = 0 @@ -50,8 +72,52 @@ Pose = tuple[list[float], list[list[float]]] +def angular_delta_deg(target: float, current: float) -> float: + """Smallest absolute angular delta for wrapped revolute joints.""" + return abs((float(target) - float(current) + 180.0) % 360.0 - 180.0) + + +def equivalent_angle_near_current_deg(target: float, current: float, *, max_abs: float) -> float: + """Choose the equivalent joint angle closest to the current controller reading.""" + target_value = float(target) + current_value = float(current) + center_step = round((current_value - target_value) / 360.0) + candidates = [target_value + 360.0 * (center_step + step) for step in range(-2, 3)] + modulo = ((target_value + 180.0) % 360.0) - 180.0 + candidates.extend( + [ + modulo, + modulo + 360.0, + modulo - 360.0, + ] + ) + if max_abs > 0.0: + bounded = [candidate for candidate in candidates if abs(candidate) <= max_abs] + if bounded: + candidates = bounded + return min(candidates, key=lambda candidate: abs(candidate - current_value)) + + 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 +126,43 @@ 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 parse_joint_index_set(raw: str, *, label: str) -> set[int]: + result: set[int] = set() + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: + continue + if item.startswith("joint_"): + item = item[6:] + elif item.startswith("j"): + item = item[1:] + try: + index = int(item) + except ValueError as exc: + raise ValueError(f"{label} contains a non-joint value: {part!r}") from exc + if not 1 <= index <= 6: + raise ValueError(f"{label} joint index must be 1..6, got {part!r}") + result.add(index - 1) + return result + + +def lock_joints_to_reference(target: list[float], reference: list[float], joint_indexes: set[int]) -> list[float]: + adjusted = list(target) + for index in joint_indexes: + adjusted[index] = reference[index] + return adjusted + + def service_name(prefix: str, suffix: str) -> str: clean_prefix = prefix.strip("/") clean_suffix = suffix.strip("/") @@ -75,6 +178,41 @@ def numeric_list(value: Any, label: str, count: int) -> list[float]: raise ValueError(f"{label} must contain only numbers") from exc +def press_contact_status(block: dict[str, Any]) -> str: + return str(block.get("press_contact_status", "")).strip() + + +def ensure_press_contact_enabled(dispenser_id: str, block: dict[str, Any]) -> None: + status = press_contact_status(block) + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + raise ValueError( + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg is marked " + f"{status!r}; refusing real press motion until PRESS{dispenser_id}_CONTACT is re-taught" + ) + + +def cup_place_status(block: dict[str, Any]) -> str: + return str(block.get("cup_place_status", "")).strip() + + +def ensure_cup_place_enabled(dispenser_id: str, block: dict[str, Any]) -> None: + status = cup_place_status(block) + if status.lower() in INVALID_CUP_PLACE_STATUSES: + raise ValueError( + f"dispenser_outlets.{dispenser_id}.cup_place_joints_deg is marked " + f"{status!r}; refusing real cup place/re-grasp motion until DISP{dispenser_id} cup place is re-taught" + ) + + +def require_dispenser_press_contact_enabled(dispenser_id: str) -> 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}") + ensure_press_contact_enabled(dispenser_id, block) + + def quaternion_to_matrix_xyzw(quaternion: list[float]) -> list[list[float]]: x, y, z, w = quaternion norm = math.sqrt(x * x + y * y + z * z + w * w) @@ -157,6 +295,518 @@ 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}") + ensure_press_contact_enabled(dispenser_id, block) + 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}") + ensure_press_contact_enabled(dispenser_id, block) + 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 load_press_pre_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}") + ensure_press_contact_enabled(dispenser_id, block) + raw_joints = block.get("press_pre_joints_deg") + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{dispenser_id}.press_pre_joints_deg", + 6, + ) + + +def load_press_common_pre_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_common_pre_joints_deg") + source_id = str(dispenser_id) + if raw_joints is None: + for candidate_id in sorted( + outlets, + key=lambda value: (0, int(value)) if str(value).isdigit() else (1, str(value)), + ): + candidate_block = outlets.get(str(candidate_id)) + if isinstance(candidate_block, dict) and candidate_block.get("press_common_pre_joints_deg") is not None: + raw_joints = candidate_block.get("press_common_pre_joints_deg") + source_id = str(candidate_id) + break + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{source_id}.press_common_pre_joints_deg", + 6, + ) + + +def load_cup_common_pre_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("cup_common_pre_joints_deg") + source_id = str(dispenser_id) + if raw_joints is None: + for candidate_id in sorted( + outlets, + key=lambda value: (0, int(value)) if str(value).isdigit() else (1, str(value)), + ): + candidate_block = outlets.get(str(candidate_id)) + if isinstance(candidate_block, dict) and candidate_block.get("cup_common_pre_joints_deg") is not None: + raw_joints = candidate_block.get("cup_common_pre_joints_deg") + source_id = str(candidate_id) + break + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{source_id}.cup_common_pre_joints_deg", + 6, + ) + + +def load_cup_pre_place_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}") + ensure_cup_place_enabled(dispenser_id, block) + raw_joints = block.get("cup_pre_place_joints_deg") + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{dispenser_id}.cup_pre_place_joints_deg", + 6, + ) + + +def load_cup_place_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}") + ensure_cup_place_enabled(dispenser_id, block) + raw_joints = block.get("cup_place_joints_deg") + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{dispenser_id}.cup_place_joints_deg", + 6, + ) + + +def load_cup_holder_target_posx(name: str) -> list[float]: + data = yaml.safe_load(CALIBRATION_CONFIG.read_text(encoding="utf-8")) or {} + holder = data.get("cup_holder") + if not isinstance(holder, dict): + raise ValueError(f"cup_holder section is missing in {CALIBRATION_CONFIG}") + block = holder.get("side_grip_place") + if not isinstance(block, dict): + raise ValueError(f"cup_holder.side_grip_place section is missing in {CALIBRATION_CONFIG}") + xyz_m = numeric_list( + block.get(f"{name}_pose_xyz_m"), + f"cup_holder.side_grip_place.{name}_pose_xyz_m", + 3, + ) + rpy_rad = numeric_list( + block.get(f"{name}_pose_rpy_rad"), + f"cup_holder.side_grip_place.{name}_pose_rpy_rad", + 3, + ) + return [ + xyz_m[0] * 1000.0, + xyz_m[1] * 1000.0, + xyz_m[2] * 1000.0, + math.degrees(rpy_rad[0]), + math.degrees(rpy_rad[1]), + math.degrees(rpy_rad[2]), + ] + + +def format_joints_deg(joints: list[float]) -> str: + return "[" + ", ".join(f"{value:.2f}" for value in joints) + "]" + + +def print_dry_run_group_detail(args: argparse.Namespace, dispenser_id: str, press_count: int) -> None: + cup_place = load_cup_place_joints_deg(dispenser_id) + cup_common_pre = load_cup_common_pre_joints_deg(dispenser_id) if args.use_cup_common_pre else None + if args.skip_release_pre: + if cup_place is not None: + print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place)}") + staging_step = "RELEASE_STAGING -> " if args.use_release_staging else "" + print( + f"[PLAN] dispenser {dispenser_id}: safe release (skip_release_pre) " + f"START_SAFE_LIFT(Z-only, >= {max(args.release_min_transit_z_m, 0.0):.3f}m) -> {staging_step}" + f"RELEASE_ABOVE(final+{max(args.release_approach_lift_m, 0.0) * 1000.0:.0f}mm) -> " + f"vertical RELEASE_FINAL -> OPEN -> " + f"POST_RELEASE_LIFT(+{max(args.post_release_safe_lift_m, 0.0) * 1000.0:.0f}mm)" + ) + elif cup_place is not None: + if cup_common_pre is not None: + print(f"[Azas] CUP_COMMON_PRE={format_joints_deg(cup_common_pre)}") + print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place)}") + 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{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" + ) + else: + print(f"[PLAN] dispenser {dispenser_id}: cup placement fallback=front_hold_poses") + + press_contact = load_press_ready_joints_deg(dispenser_id) + press_pre = ( + load_press_pre_joints_deg(dispenser_id) + if press_contact is not None and not args.skip_measured_press_pre + else None + ) + press_common_pre = load_press_common_pre_joints_deg(dispenser_id) if press_contact is not None else None + z_overdrive_mm = (max(args.press_depth_m, 0.0) + max(args.press_extra_depth_m, 0.0)) * 1000.0 + if press_contact is not None and press_pre is not None and not args.skip_measured_press_pre: + print(f"[Azas] PRESS_PRE={format_joints_deg(press_pre)}") + print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") + print( + f"[PLAN] dispenser {dispenser_id}: press PRESS_PRE " + f"-> CONTACT_ENTRY_LIFT +{max(args.press_contact_entry_lift_m, 0.0) * 1000.0:.1f}mm -> PRESS_CONTACT " + f"-> PRESS_Z_OVERDRIVE {z_overdrive_mm:.1f}mm -> PRESS_CONTACT -> PRESS_PRE x{press_count}" + ) + elif press_contact is not None: + if args.press_reset_before_press and press_common_pre is not None: + print(f"[Azas] PRESS_COMMON_PRE={format_joints_deg(press_common_pre)}") + print(f"[Azas] PRESS_CONTACT={format_joints_deg(press_contact)}") + if args.press_reset_before_press: + pre_step = "PRESS_COMMON_PRE -> " if press_common_pre is not None else "PRESS_RESET_HOME -> " + else: + pre_step = "" + print( + f"[PLAN] dispenser {dispenser_id}: press {pre_step}CONTACT_ENTRY_LIFT " + f"(CONTACT+Z{max(args.press_contact_entry_lift_m, 0.0) * 1000.0:.1f}mm) " + f"-> PRESS_CONTACT -> " + f"(PRESS_Z_OVERDRIVE {z_overdrive_mm:.1f}mm -> PRESS_CONTACT) x{press_count} " + "-> POST_PRESS_SAFE_LIFT" + ) + else: + press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) + print( + 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]]: + 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 + + +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. @@ -172,15 +822,50 @@ 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.set_current_tcp = self.node.create_client(SetCurrentTcp, service_name(args.service_prefix, "tcp/set_current_tcp")) + self.get_current_tcp = self.node.create_client(GetCurrentTcp, service_name(args.service_prefix, "tcp/get_current_tcp")) self.gripper = self.node.create_client(SetGripper, args.gripper_service) + self.previous_tcp_name: str | None = None def close(self) -> None: + self.restore_tcp_if_needed() self.node.destroy_node() 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"), + ] + if self.args.dispenser_tcp_name: + required.extend( + [ + (self.set_current_tcp, "SetCurrentTcp"), + (self.get_current_tcp, "GetCurrentTcp"), + ] + ) + 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)) + self.configure_tcp() + 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,21 +880,137 @@ def _call(self, client: Any, request: Any, *, timeout_sec: float, label: str) -> raise RuntimeError(f"{label} returned no response") return response - def current_posx(self, timeout_sec: float | None = None) -> list[float]: - req = GetCurrentPosx.Request() - req.ref = DR_BASE + def wait_motion_done(self, label: str, *, timeout_sec: float) -> None: 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]] + 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_tcp_name(self) -> str: + response = self._call( + self.get_current_tcp, + GetCurrentTcp.Request(), + timeout_sec=self.args.wait_service_sec, + label="GetCurrentTcp", + ) + if not response.success: + raise RuntimeError("GetCurrentTcp returned success=false") + return str(response.info).strip() + + def set_tcp_name(self, name: str, *, label: str) -> None: + request = SetCurrentTcp.Request() + request.name = str(name).strip() + print(f"[Azas] {label}: setting Doosan current TCP to {request.name or ''}") + response = self._call( + self.set_current_tcp, + request, + timeout_sec=self.args.wait_service_sec, + label=label, + ) + if not response.success: + raise RuntimeError(f"{label} returned success=false") + + def configure_tcp(self) -> None: + requested = str(self.args.dispenser_tcp_name).strip() + if not requested: + print("[WARN] dispenser_tcp_name is empty; keeping current Doosan TCP") + return + self.previous_tcp_name = self.current_tcp_name() + print( + "[Azas] Doosan TCP before measured recipe: " + f"{self.previous_tcp_name if self.previous_tcp_name else ''}" + ) + if self.previous_tcp_name != requested: + try: + self.set_tcp_name(requested, label="SetCurrentTcp before measured recipe") + except RuntimeError as exc: + if not self.args.allow_tcp_set_failure: + raise RuntimeError( + f"failed to set Doosan TCP '{requested}': {exc}. " + "The measured press poses require this TCP; refusing real press motion." + ) from exc + print(f"[WARN] failed to set TCP '{requested}': {exc}; continuing because allow_tcp_set_failure=true") + return + current = self.current_tcp_name() + print(f"[Azas] Doosan TCP active for measured recipe: {current if current else ''}") + if current != requested and not self.args.allow_tcp_set_failure: + raise RuntimeError( + f"requested TCP '{requested}' but controller reports '{current}'. " + "Refusing real press motion because FK/press poses would use the wrong TCP." + ) + + def restore_tcp_if_needed(self) -> None: + requested = str(self.args.dispenser_tcp_name).strip() + if not self.args.restore_tcp_after_run: + return + if self.previous_tcp_name is None or self.previous_tcp_name == requested: + return + try: + self.set_tcp_name(self.previous_tcp_name, label="SetCurrentTcp restore after measured recipe") + current = self.current_tcp_name() + print(f"[Azas] restored Doosan TCP after measured recipe: {current if current else ''}") + except RuntimeError as exc: + print(f"[WARN] failed to restore previous Doosan TCP after measured recipe: {exc}", file=sys.stderr) + + def current_posx(self, timeout_sec: float | None = None) -> list[float]: + 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]: + 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() @@ -243,6 +1044,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 = [ @@ -266,6 +1068,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] @@ -277,20 +1086,657 @@ def move_front_hold( req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE 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: + 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) + 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_posx_joint_fallback( + self, + posx_mm_deg: list[float], + *, + label: str, + velocity: float, + acceleration: float, + verify_tolerance_mm: float | None = None, + max_joint_delta_deg: float | None = None, + ) -> None: + self.validate_cartesian_target_z(posx_mm_deg, label=label) + joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") + joints_deg = self.normalize_ik_joints_near_current(joints_deg, label=label) + self.validate_ik_fallback_joints(joints_deg, label=label, max_joint_delta_deg=max_joint_delta_deg) + 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", + tolerance_mm=verify_tolerance_mm, + ) + + def measured_cup_place_posx_with_release_offset(self, joints_deg: list[float], *, label: str) -> list[float]: + posx = self.fkin_posx(joints_deg, label=f"{label} measured FK") + adjusted = list(posx) + adjusted[0] += self.args.move_release_offset_x_m * 1000.0 + adjusted[1] += self.args.move_release_offset_y_m * 1000.0 + adjusted[2] += self.args.move_release_offset_z_m * 1000.0 + print( + "[Azas] " + f"{label}: applying measured cup-place offset " + f"dx={self.args.move_release_offset_x_m * 1000.0:.1f}mm " + f"dy={self.args.move_release_offset_y_m * 1000.0:.1f}mm " + f"dz={self.args.move_release_offset_z_m * 1000.0:.1f}mm " + f"target_posx=[{adjusted[0]:.1f}, {adjusted[1]:.1f}, {adjusted[2]:.1f}, " + f"{adjusted[3]:.1f}, {adjusted[4]:.1f}, {adjusted[5]:.1f}]" + ) + return adjusted + + def move_measured_cup_place_with_offset( + self, + joints_deg: list[float], + *, + label: str, + velocity: float, + acceleration: float, + ) -> None: + if ( + abs(self.args.move_release_offset_x_m) <= 1e-6 + and abs(self.args.move_release_offset_y_m) <= 1e-6 + and abs(self.args.move_release_offset_z_m) <= 1e-6 + ): + joints_deg = self.normalize_joints_near_current(joints_deg, label=label) + self.movej(joints_deg, label=label, velocity=velocity, acceleration=acceleration) + return + target = self.measured_cup_place_posx_with_release_offset(joints_deg, label=label) + self.move_posx_joint_fallback( + target, + label=f"{label} with release offset", + velocity=velocity, + acceleration=acceleration, + ) + + def move_measured_cup_place_via_high_entry( + self, + joints_deg: list[float], + *, + dispenser_id: str, + ) -> None: + final_target = self.measured_cup_place_posx_with_release_offset( + joints_deg, + label=f"move to measured DISP{dispenser_id}_PLACE cup-place joints", + ) + high_target = list(final_target) + desired_high_z = max( + final_target[2] + max(self.args.move_prehold_offset_z_m, 0.0) * 1000.0, + max(self.args.regrasp_min_transit_z_m, 0.0) * 1000.0, + ) + max_high_z = max(self.args.regrasp_max_transit_z_m, 0.0) * 1000.0 + if max_high_z > 0.0: + desired_high_z = min(desired_high_z, max_high_z) + high_target[2] = max(final_target[2], desired_high_z) + print( + "[Azas] " + f"measured cup placement high entry: dispenser={dispenser_id} " + f"target_posx=[{high_target[0]:.1f}, {high_target[1]:.1f}, {high_target[2]:.1f}, " + f"{high_target[3]:.1f}, {high_target[4]:.1f}, {high_target[5]:.1f}]" + ) + self.move_posx_joint_fallback( + high_target, + label=f"measured DISP{dispenser_id} high entry before cup place", + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + ) + self.move_posx( + final_target, + label=f"final measured DISP{dispenser_id} cup-place descent", + velocity=self.args.move_velocity, + acceleration=self.args.move_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + + def move_measured_cup_regrasp_with_rear_entry( + self, + joints_deg: list[float], + *, + dispenser_id: str, + final_regrasp: bool = False, + ) -> None: + final_target = self.measured_cup_place_posx_with_release_offset( + joints_deg, + label=f"return to measured DISP{dispenser_id}_PLACE cup re-grasp joints", + ) + if final_regrasp: + dx_mm = self.args.final_regrasp_extra_x_offset_m * 1000.0 + dy_mm = self.args.final_regrasp_extra_y_offset_m * 1000.0 + dz_mm = self.args.final_regrasp_extra_z_offset_m * 1000.0 + if abs(dx_mm) > 1e-6 or abs(dy_mm) > 1e-6 or abs(dz_mm) > 1e-6: + final_target = list(final_target) + final_target[0] += dx_mm + final_target[1] += dy_mm + final_target[2] += dz_mm + print( + "[Azas] " + f"final cup re-grasp tighter target before holder: dispenser={dispenser_id} " + f"dx={dx_mm:.1f}mm dy={dy_mm:.1f}mm dz={dz_mm:.1f}mm " + f"target_posx=[{final_target[0]:.1f}, {final_target[1]:.1f}, {final_target[2]:.1f}, " + f"{final_target[3]:.1f}, {final_target[4]:.1f}, {final_target[5]:.1f}]" + ) + rear_target = list(final_target) + rear_target[0] += self.args.regrasp_rear_entry_offset_x_m * 1000.0 + rear_target[1] += self.args.regrasp_rear_entry_offset_y_m * 1000.0 + high_rear_target = list(rear_target) + desired_high_z = max( + final_target[2] + max(self.args.regrasp_approach_offset_z_m, 0.0) * 1000.0, + max(self.args.regrasp_min_transit_z_m, 0.0) * 1000.0, + ) + max_high_z = max(self.args.regrasp_max_transit_z_m, 0.0) * 1000.0 + if max_high_z > 0.0: + desired_high_z = min(desired_high_z, max_high_z) + high_rear_target[2] = max(rear_target[2], desired_high_z) + if ( + abs(self.args.regrasp_rear_entry_offset_x_m) > 1e-6 + or abs(self.args.regrasp_rear_entry_offset_y_m) > 1e-6 + ): + print( + "[Azas] " + f"measured cup re-grasp high rear entry: dispenser={dispenser_id} " + f"target_posx=[{high_rear_target[0]:.1f}, {high_rear_target[1]:.1f}, " + f"{high_rear_target[2]:.1f}, {high_rear_target[3]:.1f}, " + f"{high_rear_target[4]:.1f}, {high_rear_target[5]:.1f}]" + ) + self.move_posx_joint_fallback( + high_rear_target, + label=f"measured DISP{dispenser_id} high rear entry before cup re-grasp", + velocity=self.args.regrasp_approach_velocity, + acceleration=self.args.regrasp_approach_acceleration, + ) + print( + "[Azas] " + f"measured cup re-grasp lowered rear entry: dispenser={dispenser_id} " + f"dx={self.args.regrasp_rear_entry_offset_x_m * 1000.0:.1f}mm " + f"dy={self.args.regrasp_rear_entry_offset_y_m * 1000.0:.1f}mm " + f"target_posx=[{rear_target[0]:.1f}, {rear_target[1]:.1f}, {rear_target[2]:.1f}, " + f"{rear_target[3]:.1f}, {rear_target[4]:.1f}, {rear_target[5]:.1f}]" + ) + self.move_posx_joint_fallback( + rear_target, + label=f"measured DISP{dispenser_id} lowered rear entry before cup re-grasp", + velocity=self.args.pick_approach_velocity, + acceleration=self.args.pick_approach_acceleration, + ) + self.move_posx( + final_target, + label=f"final measured DISP{dispenser_id} re-grasp forward approach to cup", + velocity=self.args.pick_approach_velocity, + acceleration=self.args.pick_approach_acceleration, + timeout_sec=self.args.pick_timeout_sec, + ) + + 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") + joints_deg = self.normalize_ik_joints_near_current(joints_deg, label=label) + self.validate_ik_fallback_joints(joints_deg, label=label) + 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 validate_cartesian_target_z(self, pos_mm_deg: list[float], *, label: str) -> None: + min_allowed_mm = max(self.args.min_allowed_tcp_z_m, 0.0) * 1000.0 + if pos_mm_deg[2] < min_allowed_mm - 1e-6: + raise RuntimeError( + f"unsafe cartesian target for {label}: " + f"target_z={pos_mm_deg[2] / 1000.0:.3f}m < min_allowed_tcp_z={min_allowed_mm / 1000.0:.3f}m; " + "no robot command sent" + ) + + def ensure_start_safe_pose(self) -> None: + """Read the live TCP pose and lift straight up before the first XY waypoint.""" + try: + pose = self.current_posx(timeout_sec=self.args.wait_service_sec) + except RuntimeError as exc: + raise RuntimeError( + f"start TCP pose read failed; aborting before any motion: {exc}" + ) from exc + current_z_m = pose[2] / 1000.0 + min_allowed_z_m = max(self.args.min_allowed_tcp_z_m, 0.0) + start_lift_z_m = max(self.args.start_safe_lift_z_m, min_allowed_z_m, 0.0) + print( + "[Azas] start TCP pose: " + f"posx=[{pose[0]:.1f}, {pose[1]:.1f}, {pose[2]:.1f}, " + f"{pose[3]:.1f}, {pose[4]:.1f}, {pose[5]:.1f}] " + f"current_z={current_z_m:.3f}m start_safe_lift_z={start_lift_z_m:.3f}m " + f"min_allowed_tcp_z={min_allowed_z_m:.3f}m " + f"force_start_safe_lift={self.args.force_start_safe_lift}" + ) + if current_z_m < min_allowed_z_m: + print( + f"[WARN] live TCP Z {current_z_m:.3f}m is below min_allowed_tcp_z " + f"{min_allowed_z_m:.3f}m; XY motion stays blocked until the Z-only " + "start safe lift completes" + ) + if not self.args.force_start_safe_lift and current_z_m >= start_lift_z_m: + print( + "[Azas] start safe lift skipped: live TCP already at or above " + f"start_safe_lift_z={start_lift_z_m:.3f}m and force_start_safe_lift=false" + ) + return + self.safe_lift_current( + label="START_SAFE_LIFT Z-only lift from live TCP pose before first waypoint", + min_z_m=start_lift_z_m, + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + + def safe_lift_current( + self, + *, + label: str, + min_z_m: float, + velocity: float, + acceleration: float, + timeout_sec: float, + verify_tolerance_mm: float | None = None, + ) -> 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]] + safe_lift_tolerance_mm = ( + self.args.safe_lift_target_tolerance_mm + if verify_tolerance_mm is None + else verify_tolerance_mm + ) + try: + self.move_posx( + target, + label=label, + velocity=velocity, + acceleration=acceleration, + timeout_sec=timeout_sec, + verify_tolerance_mm=safe_lift_tolerance_mm, + ) + 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, + verify_tolerance_mm=safe_lift_tolerance_mm, + ) + + def move_posx( + self, + pos: list[float], + *, + label: str, + velocity: float, + acceleration: float, + timeout_sec: float, + verify_tolerance_mm: float | None = None, + ) -> None: + self.validate_cartesian_target_z(pos, label=label) + 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_for_target(pos, label=label) + self.wait_motion_done(label, timeout_sec=timeout_sec) + self.wait_for_target(pos, label=label, tolerance_mm=verify_tolerance_mm) + + def move_generated_press_pre(self, pos: list[float], *, label: str) -> None: + if self.args.press_generated_pre_use_joint: + print( + f"[Azas] {label}: using IK MoveJoint for generated press entry approach; " + "CONTACT/PRESS stroke remains Z-only MoveLine" + ) + self.move_posx_joint_fallback( + pos, + label=label, + velocity=self.args.press_contact_joint_velocity, + acceleration=self.args.press_contact_joint_acceleration, + ) + return + try: + self.move_posx( + pos, + label=label, + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + except RuntimeError as exc: + if not self.args.press_generated_pre_joint_fallback: + raise + print( + f"[WARN] MoveLine/verification failed for {label}: {exc}; " + "retrying the same generated press entry target with IK MoveJoint fallback" + ) + self.move_posx_joint_fallback( + pos, + label=label, + velocity=self.args.press_contact_joint_velocity, + acceleration=self.args.press_contact_joint_acceleration, + ) + + def move_posx_no_verify( + self, + pos: list[float], + *, + label: str, + velocity: float, + acceleration: float, + timeout_sec: float, + ) -> None: + self.validate_cartesian_target_z(pos, label=label) + 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] " + + 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 normalize_ik_joints_near_current(self, joints_deg: list[float], *, label: str) -> list[float]: + """Prevent wrist wrap by commanding the nearest equivalent joint branch.""" + current = self.current_posj(timeout_sec=5.0) + max_abs = max(float(self.args.ik_fallback_max_abs_joint_deg), 0.0) + normalized = [ + equivalent_angle_near_current_deg(target, current_value, max_abs=max_abs) + for target, current_value in zip(joints_deg, current, strict=True) + ] + changed = [ + f"joint_{index + 1} {before:.1f}->{after:.1f}deg current={current[index]:.1f}" + for index, (before, after) in enumerate(zip(joints_deg, normalized, strict=True)) + if abs(before - after) > 1.0 + ] + if changed: + print( + "[Azas] " + f"{label}: normalized joints near current to avoid wrist wrap: " + + "; ".join(changed) + ) + return normalized + + def normalize_joints_near_current(self, joints_deg: list[float], *, label: str) -> list[float]: + if self.args.normalize_measured_joints_near_current: + return self.normalize_ik_joints_near_current(joints_deg, label=label) + print( + "[Azas] " + f"{label}: using measured joint values exactly; no 360deg wrap normalization" + ) + return list(joints_deg) + + def validate_ik_fallback_joints( + self, + joints_deg: list[float], + *, + label: str, + max_joint_delta_deg: float | None = None, + ) -> 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" + ) + delta_limit = self.args.ik_fallback_max_joint_delta_deg if max_joint_delta_deg is None else max_joint_delta_deg + max_delta = max(float(delta_limit), 0.0) + if max_delta <= 0.0: + return + current = self.current_posj(timeout_sec=5.0) + deltas = [angular_delta_deg(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 validate_press_contact_joints( + self, + dispenser_id: str, + joints_deg: list[float], + configured_xyz_mm: list[float], + ) -> None: + tolerance_mm = max(float(self.args.press_contact_joint_pose_tolerance_mm), 0.0) + if tolerance_mm <= 0.0: + return + try: + fk_posx = self.fkin_posx(joints_deg, label=f"press contact joint sanity dispenser {dispenser_id}") + except RuntimeError as exc: + if self.args.strict_press_contact_joint_pose_match: + raise + print( + f"[WARN] press contact joint sanity skipped for dispenser {dispenser_id}: {exc}; " + "continuing because measured press_contact_joints_deg are authoritative" + ) + return + distance_mm = math.dist(fk_posx[:3], configured_xyz_mm) + print( + "[Azas] press contact joint sanity: " + f"dispenser={dispenser_id} configured_xyz_mm=[{configured_xyz_mm[0]:.1f}, " + f"{configured_xyz_mm[1]:.1f}, {configured_xyz_mm[2]:.1f}] " + f"fk_xyz_mm=[{fk_posx[0]:.1f}, {fk_posx[1]:.1f}, {fk_posx[2]:.1f}] " + f"distance={distance_mm:.1f}mm tolerance={tolerance_mm:.1f}mm" + ) + if distance_mm <= tolerance_mm: + return + if self.args.strict_press_contact_joint_pose_match and not self.args.allow_press_contact_joint_pose_mismatch: + raise RuntimeError( + f"press_contact_joints_deg for dispenser {dispenser_id} do not match " + f"press_pose_xyz_m: FK distance={distance_mm:.1f}mm exceeds " + f"{tolerance_mm:.1f}mm. Refusing real press motion; re-teach " + "press_contact_joints_deg or run a non-motion preview." + ) + print( + f"[WARN] press_contact_joints_deg/FK mismatch for dispenser {dispenser_id}: " + f"distance={distance_mm:.1f}mm exceeds {tolerance_mm:.1f}mm; continuing because " + "measured press_contact_joints_deg are authoritative on this controller/TCP setup" + ) - def wait_for_target(self, target_pos_mm_deg: list[float], *, label: str) -> None: + 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(angular_delta_deg(target_joints_deg[index], actual[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 + 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") + + def wait_for_target( + self, + target_pos_mm_deg: list[float], + *, + label: str, + tolerance_mm: float | None = None, + ) -> 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() + tolerance = max(float(self.args.target_tolerance_mm if tolerance_mm is None else tolerance_mm), 0.1) 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): + print(f"[Azas] verify {label}: distance={last_distance:.1f}mm tolerance={tolerance:.1f}mm") + if last_distance <= tolerance: return - time.sleep(1.0) + 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, tolerance) + 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") def gripper_command(self, command: str, *, width_m: float, force_n: float, label: str) -> None: @@ -302,8 +1748,367 @@ 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 validate_cup_holder_target(self, posx_mm_deg: list[float], *, label: str) -> None: + self.validate_cartesian_target_z(posx_mm_deg, label=label) + x_m = posx_mm_deg[0] / 1000.0 + y_m = posx_mm_deg[1] / 1000.0 + z_m = posx_mm_deg[2] / 1000.0 + if not (self.args.cup_holder_x_min_m <= x_m <= self.args.cup_holder_x_max_m): + raise RuntimeError( + f"cup-holder target {label} rejected: x={x_m:.3f}m outside " + f"[{self.args.cup_holder_x_min_m:.3f}, {self.args.cup_holder_x_max_m:.3f}]" + ) + if not (self.args.cup_holder_y_min_m <= y_m <= self.args.cup_holder_y_max_m): + raise RuntimeError( + f"cup-holder target {label} rejected: y={y_m:.3f}m outside " + f"[{self.args.cup_holder_y_min_m:.3f}, {self.args.cup_holder_y_max_m:.3f}]" + ) + if not (self.args.cup_holder_z_min_m <= z_m <= self.args.cup_holder_z_max_m): + raise RuntimeError( + f"cup-holder target {label} rejected: z={z_m:.3f}m outside " + f"[{self.args.cup_holder_z_min_m:.3f}, {self.args.cup_holder_z_max_m:.3f}]" + ) + + 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[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 ( + ("CUP_HOLDER_PRE_PLACE", pre_place), + ("CUP_HOLDER_PLACE_FINAL", place_final), + ("CUP_HOLDER_RETREAT", retreat), + ): + self.validate_cup_holder_target(posx, label=label) + print( + f"[Azas] {label}: posx=[{posx[0]:.1f}, {posx[1]:.1f}, {posx[2]:.1f}, " + f"{posx[3]:.1f}, {posx[4]:.1f}, {posx[5]:.1f}]" + ) + print( + "[Azas] cup holder place path: " + "CUP_HOLDER_PRE_PLACE -> CUP_HOLDER_PLACE_FINAL -> RG2_OPEN -> CUP_HOLDER_RETREAT" + ) + self.move_posx( + pre_place, + label="CUP_HOLDER_PRE_PLACE measured side-grip approach", + velocity=self.args.cup_holder_approach_velocity, + acceleration=self.args.cup_holder_approach_acceleration, + timeout_sec=self.args.cup_holder_timeout_sec, + verify_tolerance_mm=self.args.cup_holder_target_tolerance_mm, + ) + self.move_posx( + place_final, + label="CUP_HOLDER_PLACE_FINAL measured side-grip place", + velocity=self.args.cup_holder_place_velocity, + acceleration=self.args.cup_holder_place_acceleration, + timeout_sec=self.args.cup_holder_timeout_sec, + verify_tolerance_mm=self.args.cup_holder_target_tolerance_mm, + ) + self.gripper_command( + "open", + width_m=self.args.gripper_open_width_m, + force_n=self.args.gripper_open_force_n, + label="RG2 full-open at cup holder", + ) + self.move_posx( + retreat, + label="CUP_HOLDER_RETREAT measured side-grip retreat", + velocity=self.args.cup_holder_retreat_velocity, + acceleration=self.args.cup_holder_retreat_acceleration, + timeout_sec=self.args.cup_holder_timeout_sec, + verify_tolerance_mm=self.args.cup_holder_target_tolerance_mm, + ) + + def _log_release_pose(self, name: str, pos: list[float], *, min_required_z_mm: float) -> None: + z_ok = pos[2] >= min_required_z_mm - 1e-6 + print( + f"[Azas] release pose {name}: " + f"posx=[{pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}, {pos[3]:.1f}, {pos[4]:.1f}, {pos[5]:.1f}] " + f"z={pos[2] / 1000.0:.3f}m min_required_z={min_required_z_mm / 1000.0:.3f}m z_ok={z_ok}" + ) + if not z_ok: + raise RuntimeError( + f"release pose {name} failed z validation: " + f"z={pos[2] / 1000.0:.3f}m < min_required_z={min_required_z_mm / 1000.0:.3f}m; " + "no release command sent" + ) + + def release_final_posx(self, dispenser_id: str) -> list[float]: + """Release final pose (unchanged from the legacy release target).""" + cup_place_joints = load_cup_place_joints_deg(dispenser_id) + if cup_place_joints is not None: + print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place_joints)}") + return self.measured_cup_place_posx_with_release_offset( + cup_place_joints, + label=f"safe release measured DISP{dispenser_id}_PLACE", + ) + position, quaternion, raw_zyz = load_front_hold_pose(self.args.config, dispenser_id) + link6_position = [ + position[0] + self.args.move_release_offset_x_m, + position[1] + self.args.move_release_offset_y_m, + position[2] + self.args.move_release_offset_z_m, + ] + move_position, move_rotation = self.compensate_current_tcp(link6_position, quaternion) + move_zyz = matrix_to_doosan_zyz_deg(move_rotation) + print( + f"[Azas] safe release final from front_hold fallback: dispenser={dispenser_id} " + f"link6_target_m=[{link6_position[0]:.4f}, {link6_position[1]:.4f}, {link6_position[2]:.4f}] " + f"raw_zyz_deg=[{raw_zyz[0]:.2f}, {raw_zyz[1]:.2f}, {raw_zyz[2]:.2f}]" + ) + return [move_position[0] * 1000.0, move_position[1] * 1000.0, move_position[2] * 1000.0, *move_zyz] + + def _release_transit_move(self, pos: list[float], *, label: str) -> None: + try: + self.move_posx( + pos, + label=label, + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + timeout_sec=self.args.move_timeout_sec, + verify_tolerance_mm=self.args.safe_lift_target_tolerance_mm, + ) + except RuntimeError as exc: + if not self.args.safe_lift_joint_fallback: + raise + print( + f"[WARN] MoveLine failed for {label}: {exc}; " + "retrying the same target with IK MoveJoint fallback" + ) + self.move_posx_joint_fallback( + pos, + label=label, + velocity=self.args.safe_lift_joint_fallback_velocity, + acceleration=self.args.safe_lift_joint_fallback_acceleration, + verify_tolerance_mm=self.args.safe_lift_target_tolerance_mm, + ) + + def safe_release_cup(self, dispenser_id: str) -> None: + """Posture-independent cup release. + + Converges from any joint state / TCP position: Z-only lift from the live + pose, transit at a safe height, then a vertical-only descent onto the + unchanged release final pose. Never moves directly from the current pose + to the release final. + """ + print( + f"[Azas] safe cup release: dispenser={dispenser_id} " + f"skip_release_pre={self.args.skip_release_pre} " + f"use_release_staging={self.args.use_release_staging}" + ) + try: + current = self.current_posx(timeout_sec=self.args.wait_service_sec) + except RuntimeError as exc: + raise RuntimeError( + f"release start TCP pose read failed; no release command sent: {exc}" + ) from exc + print( + "[Azas] release current TCP pose: " + f"posx=[{current[0]:.1f}, {current[1]:.1f}, {current[2]:.1f}, " + f"{current[3]:.1f}, {current[4]:.1f}, {current[5]:.1f}]" + ) + min_allowed_mm = max(self.args.min_allowed_tcp_z_m, 0.0) * 1000.0 + min_transit_mm = max(self.args.release_min_transit_z_m, 0.0) * 1000.0 + + final_posx = self.release_final_posx(dispenser_id) + above_posx = list(final_posx) + above_posx[2] = final_posx[2] + max(self.args.release_approach_lift_m, 0.0) * 1000.0 + above_high_posx = list(above_posx) + above_high_posx[2] = max(above_posx[2], min_transit_mm) + staging_posx: list[float] | None = None + if self.args.use_release_staging: + staging_posx = list(above_high_posx) + if self.args.release_staging_x_m is not None: + staging_posx[0] = self.args.release_staging_x_m * 1000.0 + if self.args.release_staging_y_m is not None: + staging_posx[1] = self.args.release_staging_y_m * 1000.0 + if self.args.release_staging_z_m is not None: + staging_posx[2] = self.args.release_staging_z_m * 1000.0 + start_lift_posx = list(current) + start_lift_posx[2] = max( + current[2] + max(self.args.release_start_safe_lift_m, 0.0) * 1000.0, + min_transit_mm, + ) + post_release_posx = list(final_posx) + post_release_posx[2] = final_posx[2] + max(self.args.post_release_safe_lift_m, 0.0) * 1000.0 + + # Validate every pose before sending any robot command. XY-transit poses + # must stay at or above the release transit height; the vertical descent + # chain (above -> final) and the post-release lift only need the global + # TCP Z floor. + self._log_release_pose("release_start_safe_lift", start_lift_posx, min_required_z_mm=min_transit_mm) + if staging_posx is not None: + self._log_release_pose("release_staging", staging_posx, min_required_z_mm=min_transit_mm) + else: + print("[Azas] release staging pose disabled (--no-use-release-staging)") + self._log_release_pose("release_above_high_entry", above_high_posx, min_required_z_mm=min_transit_mm) + self._log_release_pose("release_above", above_posx, min_required_z_mm=min_allowed_mm) + self._log_release_pose("release_final", final_posx, min_required_z_mm=min_allowed_mm) + self._log_release_pose("post_release_lift", post_release_posx, min_required_z_mm=min_allowed_mm) + if above_posx[2] < final_posx[2] - 1e-6: + raise RuntimeError( + "release_above pose ended up below release_final; check --release-approach-lift-m; " + "no release command sent" + ) + + self._release_transit_move( + start_lift_posx, + label="release start vertical safe lift (Z-only from live pose)", + ) + if staging_posx is not None and any( + abs(staging_posx[index] - above_high_posx[index]) > 1.0 for index in range(3) + ): + self._release_transit_move(staging_posx, label="release staging pose") + self._release_transit_move( + above_high_posx, + label="release high entry directly above release final", + ) + if above_high_posx[2] - above_posx[2] > 1.0: + self.move_posx( + above_posx, + label="vertical descent to release_above", + velocity=self.args.move_velocity, + acceleration=self.args.move_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + + # Final descent is allowed only as a vertical drop from release_above: + # the live pose must already match release_above in X/Y/orientation. + live = self.current_posx(timeout_sec=self.args.wait_service_sec) + xy_tolerance_mm = max(self.args.target_tolerance_mm, 20.0) + orientation_tolerance_deg = 5.0 + xy_error_mm = max(abs(live[0] - above_posx[0]), abs(live[1] - above_posx[1])) + orientation_error_deg = max( + abs(angular_delta_deg(above_posx[index], live[index])) for index in (3, 4, 5) + ) + if xy_error_mm > xy_tolerance_mm or orientation_error_deg > orientation_tolerance_deg: + raise RuntimeError( + "release final descent blocked: live pose is not vertically above release final " + f"(xy_error={xy_error_mm:.1f}mm tol={xy_tolerance_mm:.1f}mm, " + f"orientation_error={orientation_error_deg:.2f}deg tol={orientation_tolerance_deg:.2f}deg); " + "no descent command sent" + ) + self.move_posx( + final_posx, + label="vertical descent to release_final", + velocity=self.args.move_velocity, + acceleration=self.args.move_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + self.gripper_command( + "open", + width_m=self.args.gripper_open_width_m, + 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") + self.move_posx( + post_release_posx, + label="post-release vertical lift above released cup", + velocity=self.args.pick_lift_velocity, + acceleration=self.args.pick_lift_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) def move_and_release(self, dispenser_id: str) -> None: + if self.args.skip_release_pre: + self.safe_release_cup(dispenser_id) + return + cup_common_pre_joints = load_cup_common_pre_joints_deg(dispenser_id) if self.args.use_cup_common_pre else None + cup_place_joints = load_cup_place_joints_deg(dispenser_id) + if cup_place_joints is not None: + if cup_common_pre_joints is not None: + print(f"[Azas] CUP_COMMON_PRE={format_joints_deg(cup_common_pre_joints)}") + print(f"[Azas] DISP_PLACE={format_joints_deg(cup_place_joints)}") + final_target = self.measured_cup_place_posx_with_release_offset( + cup_place_joints, + label=f"DISP{dispenser_id}_PLACE source measured DISP_PLACE", + ) + if cup_common_pre_joints is not None: + print( + f"[Azas] cup placement: dispenser={dispenser_id} using " + "CUP_COMMON_PRE -> DISP_PLACE; saved cup_pre_place_joints_deg ignored" + ) + self.movej( + cup_common_pre_joints, + label="CUP_COMMON_PRE common cup-place pre joints", + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + ) + else: + print( + 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] += 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={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}]" + ) + pre_label = f"generated DISP{dispenser_id}_PRE from DISP_PLACE X/Z offset" + if self.args.generated_cup_pre_use_joint_fallback: + print( + f"[Azas] {pre_label}: using IK MoveJoint fallback because " + "generated_cup_pre_use_joint_fallback=true" + ) + self.move_posx_joint_fallback( + pre_target, + label=pre_label, + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + max_joint_delta_deg=self.args.generated_cup_pre_max_joint_delta_deg, + ) + else: + self.move_posx( + pre_target, + label=pre_label, + velocity=self.args.move_prehold_velocity, + acceleration=self.args.move_prehold_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + self.move_posx( + final_target, + label=f"DISP_PLACE measured DISP{dispenser_id} cup release", + velocity=self.args.move_velocity, + acceleration=self.args.move_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + self.gripper_command( + "open", + width_m=self.args.gripper_open_width_m, + 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") + return + stages = [ ( "pre-hold", @@ -313,8 +2118,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: @@ -335,30 +2154,168 @@ def move_and_release(self, dispenser_id: str) -> None: "open", width_m=self.args.gripper_open_width_m, force_n=self.args.gripper_open_force_n, - label="RG2 full-open release", + 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, *, final_regrasp: bool = False) -> None: + grasp_width_m = ( + self.args.final_regrasp_grasp_width_m + if final_regrasp + else self.args.gripper_grasp_width_m + ) + grasp_force_n = self.args.final_regrasp_force_n if final_regrasp else self.args.gripper_force_n + grasp_label = "RG2 tight final side-grasp before cup holder" if final_regrasp else "RG2 soft side-grasp" + if self.args.regrasp_reset_before_cup: + print( + "[Azas] re-grasp transit: moving through HOME joint waypoint before returning to cup; " + "skipping post-press Cartesian high-Z lift" + ) + self.movej( + list(self.args.regrasp_reset_joints_deg), + label="move to re-grasp HOME joints before cup return", + velocity=self.args.regrasp_reset_joint_velocity, + acceleration=self.args.regrasp_reset_joint_acceleration, + ) + else: + self.safe_lift_current( + label="safe vertical lift after press before re-grasp transit", + min_z_m=self.args.post_press_safe_lift_z_m, + velocity=self.args.regrasp_approach_velocity, + acceleration=self.args.regrasp_approach_acceleration, + timeout_sec=self.args.move_timeout_sec, + verify_tolerance_mm=self.args.post_press_safe_lift_target_tolerance_mm, + ) + if ( + not self.args.regrasp_reset_before_cup + and (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 robot-side X 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, + verify_tolerance_mm=self.args.safe_lift_target_tolerance_mm, + ) + cup_place_joints = load_cup_place_joints_deg(dispenser_id) + if cup_place_joints is not None: + print( + f"[Azas] cup re-grasp: dispenser={dispenser_id} using measured " + "DISP_PLACE with generated high rear-entry; not front_hold_poses" + ) + self.gripper_command( + "open", + width_m=self.args.gripper_open_width_m, + force_n=self.args.gripper_open_force_n, + label="RG2 open at safe robot-side retreat before measured cup re-grasp", + ) + self.move_measured_cup_regrasp_with_rear_entry( + cup_place_joints, + dispenser_id=dispenser_id, + final_regrasp=final_regrasp, + ) + self.gripper_command( + "set_width", + width_m=grasp_width_m, + force_n=grasp_force_n, + label=grasp_label, + ) + pose = self.current_posx() + target = [pose[0], pose[1], pose[2] + max(self.args.pick_lift_m, 0.0) * 1000.0, pose[3], pose[4], pose[5]] + req = MoveLine.Request() + req.pos = target + req.vel = [self.args.pick_lift_velocity, self.args.pick_lift_velocity] + req.acc = [self.args.pick_lift_acceleration, self.args.pick_lift_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=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") + return + + front_hold_position, _, _ = load_front_hold_pose(self.args.config, dispenser_id) + final_offset_x_m = self.args.move_release_offset_x_m + final_offset_y_m = self.args.move_release_offset_y_m + final_offset_z_m = self.args.move_release_offset_z_m + if final_regrasp: + final_offset_x_m += self.args.final_regrasp_extra_x_offset_m + final_offset_y_m += self.args.final_regrasp_extra_y_offset_m + final_offset_z_m += self.args.final_regrasp_extra_z_offset_m + print( + "[Azas] " + f"final front-hold re-grasp tighter offset before holder: dispenser={dispenser_id} " + f"offset_x={final_offset_x_m:.3f}m offset_y={final_offset_y_m:.3f}m " + f"offset_z={final_offset_z_m:.3f}m" + ) + released_hold_z_m = front_hold_position[2] + final_offset_z_m + desired_approach_z_m = max( + 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)) + 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) + rear_offset_x_m = final_offset_x_m + self.args.regrasp_rear_entry_offset_x_m + rear_offset_y_m = final_offset_y_m + self.args.regrasp_rear_entry_offset_y_m + self.move_front_hold( + dispenser_id, + label="re-grasp high transit above rear entry", + offset_x_m=rear_offset_x_m, + offset_y_m=rear_offset_y_m, + 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_open_force_n, + label="RG2 open at re-grasp high rear entry", ) - - def regrasp_and_lift(self, dispenser_id: str) -> None: - 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", + self.move_front_hold( + dispenser_id, + label="re-grasp lowered rear entry before forward approach", + offset_x_m=rear_offset_x_m, + offset_y_m=rear_offset_y_m, + offset_z_m=final_offset_z_m, + velocity=self.args.pick_approach_velocity, + acceleration=self.args.pick_approach_acceleration, ) 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, + label="final re-grasp forward approach to cup", + offset_x_m=final_offset_x_m, + offset_y_m=final_offset_y_m, + offset_z_m=final_offset_z_m, velocity=self.args.pick_approach_velocity, acceleration=self.args.pick_approach_acceleration, ) self.gripper_command( "set_width", - width_m=self.args.gripper_grasp_width_m, - force_n=self.args.gripper_force_n, - label="RG2 soft side-grasp", + width_m=grasp_width_m, + force_n=grasp_force_n, + label=grasp_label, ) pose = self.current_posx() target = [pose[0], pose[1], pose[2] + max(self.args.pick_lift_m, 0.0) * 1000.0, pose[3], pose[4], pose[5]] @@ -375,8 +2332,607 @@ 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: + current_pose = self.current_posx() + press_drop_m = max(self.args.press_depth_m, 0.0) + max(self.args.press_extra_depth_m, 0.0) + contact_joints = None if self.args.force_cartesian_press else load_press_ready_joints_deg(dispenser_id) + skip_measured_press_pre = bool(self.args.skip_measured_press_pre) + pre_joints = ( + None + if self.args.force_cartesian_press or skip_measured_press_pre + else load_press_pre_joints_deg(dispenser_id) + ) + press_common_pre_joints = None if self.args.force_cartesian_press else load_press_common_pre_joints_deg(dispenser_id) + + if contact_joints is not None and (skip_measured_press_pre or pre_joints is not None): + print(f"[Azas] skip_measured_press_pre={str(skip_measured_press_pre).lower()}") + if not skip_measured_press_pre: + print(f"[Azas] PRESS_PRE={format_joints_deg(pre_joints)}") + if skip_measured_press_pre and self.args.press_reset_before_press and press_common_pre_joints is not None: + print(f"[Azas] PRESS_COMMON_PRE={format_joints_deg(press_common_pre_joints)}") + print(f"[Azas] PRESS_CONTACT={format_joints_deg(contact_joints)}") + common_pre_path = "" + if skip_measured_press_pre and self.args.press_reset_before_press: + common_pre_path = "PRESS_COMMON_PRE -> " if press_common_pre_joints is not None else "PRESS_RESET_HOME -> " + if skip_measured_press_pre: + print( + "[Azas] measured press path: " + f"SAFE_LIFT -> {common_pre_path}CONTACT_ENTRY_LIFT -> PRESS_CONTACT -> " + f"(PRESS_Z_OVERDRIVE -> PRESS_CONTACT) x{max(int(press_count), 1)} -> " + "POST_PRESS_SAFE_LIFT -> REGRASP" + ) + else: + print( + "[Azas] measured press path: " + "SAFE_LIFT -> PRESS_PRE -> CONTACT_ENTRY_LIFT -> PRESS_CONTACT -> " + "PRESS_Z_OVERDRIVE -> PRESS_CONTACT -> PRESS_PRE -> REGRASP" + ) + 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_LIFT pre-step robot-side X retreat away from dispenser before press", + 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) + transit_z = max( + current_pose[2] + max(self.args.press_transit_height_m, 0.0) * 1000.0, + max(self.args.press_min_transit_z_m, 0.0) * 1000.0, + ) + 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 CONTACT_ENTRY_LIFT" + if skip_measured_press_pre + else "SAFE_LIFT away from released cup before measured PRESS_PRE" + ), + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + verify_tolerance_mm=self.args.safe_lift_target_tolerance_mm, + ) + 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 skip_measured_press_pre and self.args.press_reset_before_press: + if press_common_pre_joints is not None: + reset_joints = list(press_common_pre_joints) + reset_label = "PRESS_COMMON_PRE common contact pre joints before CONTACT_ENTRY_LIFT" + else: + reset_joints = list(self.args.press_reset_joints_deg) + reset_label = "move to press HOME joints before CONTACT_ENTRY_LIFT" + self.movej( + reset_joints, + label=reset_label, + velocity=self.args.press_reset_joint_velocity, + acceleration=self.args.press_reset_joint_acceleration, + ) + + if not skip_measured_press_pre: + pre_joints = self.normalize_joints_near_current( + list(pre_joints), + label="PRESS_PRE measured joints", + ) + self.movej( + pre_joints, + label="PRESS_PRE measured press pre-contact joints", + velocity=self.args.press_pre_joint_velocity, + acceleration=self.args.press_pre_joint_acceleration, + ) + contact_joints = list(contact_joints) + raw_contact_joints = list(contact_joints) + if self.args.press_lock_contact_joint_indexes: + if skip_measured_press_pre: + print( + "[WARN] press contact joint lock ignored because " + "skip_measured_press_pre=true and measured pre-contact joints are not commanded" + ) + else: + contact_joints = lock_joints_to_reference( + contact_joints, + pre_joints, + self.args.press_lock_contact_joint_indexes, + ) + if contact_joints != raw_contact_joints: + print( + "[Azas] press contact joint lock: raw=[" + + ", ".join(f"{value:.2f}" for value in raw_contact_joints) + + "] command=[" + + ", ".join(f"{value:.2f}" for value in contact_joints) + + "] locked_joints=" + + ",".join(str(index + 1) for index in sorted(self.args.press_lock_contact_joint_indexes)) + ) + contact_joints = self.normalize_joints_near_current( + contact_joints, + label="PRESS_CONTACT measured joints", + ) + # Generated entry-lift pose directly above the measured contact: every + # contact approach must pass through it so the press always enters from + # straight above instead of from an arbitrary PRESS_PRE direction. + entry_lift_m = max(self.args.press_contact_entry_lift_m, 0.0) + contact_fk_posx = self.fkin_posx( + 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( + contact_entry_posx, + label=f"generated PRESS_CONTACT entry lift dispenser {dispenser_id}", + ) + print( + "[Azas] integrated press: " + f"dispenser={dispenser_id} count={press_count} " + f"PRESS_CONTACT={format_joints_deg(contact_joints)} " + f"contact_entry_z={contact_entry_posx[2] / 1000.0:.3f}m " + 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; " + 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)}" + ) + ) + if skip_measured_press_pre: + self.move_generated_press_pre( + contact_entry_posx, + 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", + 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 "" + if press_drop_m > 0.0: + extra_press_posx = list(contact_posx) + extra_press_posx[2] -= press_drop_m * 1000.0 + self.move_posx( + extra_press_posx, + label=f"PRESS_Z_OVERDRIVE from stable measured contact{suffix}", + 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), + ) + if self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + self.move_posx( + contact_posx, + label=f"return to stable PRESS_CONTACT after Z overdrive{suffix}", + 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), + ) + elif self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + self.move_posx( + contact_entry_posx, + label="POST_PRESS_SAFE_LIFT above measured PRESS_CONTACT", + 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), + ) + if self.args.press_post_retreat_wait_seconds > 0.0: + time.sleep(self.args.press_post_retreat_wait_seconds) + return + + for press_index in range(1, max(int(press_count), 1) + 1): + suffix = f" {press_index}/{press_count}" if press_count > 1 else "" + self.move_generated_press_pre( + contact_entry_posx, + label=f"CONTACT_ENTRY_LIFT above measured PRESS_CONTACT{suffix}", + ) + self.movej( + contact_joints, + label=f"PRESS_CONTACT measured contact joints{suffix}", + velocity=self.args.press_contact_joint_velocity, + acceleration=self.args.press_contact_joint_acceleration, + ) + if press_drop_m > 0.0: + contact_posx = self.current_posx(timeout_sec=self.args.wait_service_sec) + extra_press_posx = list(contact_posx) + extra_press_posx[2] -= press_drop_m * 1000.0 + self.move_posx( + extra_press_posx, + label=f"PRESS_Z_OVERDRIVE from measured contact{suffix}", + 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), + ) + if self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + self.move_posx( + contact_posx, + label=f"return to PRESS_CONTACT after Z overdrive{suffix}", + 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), + ) + elif self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + if skip_measured_press_pre: + self.move_posx( + contact_entry_posx, + label=f"POST_PRESS_SAFE_LIFT above measured PRESS_CONTACT{suffix}", + 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), + ) + else: + self.movej( + pre_joints, + label=f"return to PRESS_PRE measured press pre-contact joints{suffix}", + velocity=self.args.press_pre_joint_velocity, + acceleration=self.args.press_pre_joint_acceleration, + ) + if self.args.press_post_retreat_wait_seconds > 0.0: + time.sleep(self.args.press_post_retreat_wait_seconds) + return + + press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) + joint_space_press = contact_joints is not None + if contact_joints is None: + 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" + ) + 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}: loading measured press contact joints " + f"(joint_6/link_6={contact_joints[5]:.2f}deg)" + ) + contact_fk_posx = self.fkin_posx( + 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 + # taught press pose. PRE and the pump stroke are generated by + # changing only Z from the FK contact pose. + generated_pre_lift_m = max(self.args.press_contact_entry_lift_m, 0.0) + pre_z = contact_z + generated_pre_lift_m * 1000.0 + transit_z = max( + current_pose[2] + max(self.args.press_transit_height_m, generated_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 - press_drop_m * 1000.0 + print( + "[Azas] integrated press: " + f"dispenser={dispenser_id} count={press_count} " + f"contact_fk=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " + f"generated_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=measured PRESS_CONTACT only; PRE/PRESS are Z-only" + ) + else: + pre_z = contact_z + max(self.args.press_pre_lift_m, 0.0) * 1000.0 + pressed_z = contact_z - press_drop_m * 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}" + ) + 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 robot-side X 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], + 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, + verify_tolerance_mm=self.args.safe_lift_target_tolerance_mm, + ) + 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: + reset_joints = list(self.args.press_reset_joints_deg) + self.movej( + reset_joints, + label="move to press HOME joints before fallback generated PRESS_PRE", + velocity=self.args.press_reset_joint_velocity, + acceleration=self.args.press_reset_joint_acceleration, + ) + + if joint_space_press and pre_joints is None: + contact_posx = [x_mm, y_mm, contact_z, rx, ry, rz] + generated_pre_posx = list(contact_posx) + generated_pre_posx[2] = pre_z + print( + "[Azas] contact-only press path: " + f"dispenser={dispenser_id} contact_posx=[{x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}, " + f"{rx:.1f}, {ry:.1f}, {rz:.1f}] " + f"generated_pre_z={pre_z:.1f} press_drop_mm={press_drop_m * 1000.0:.1f}" + ) + self.move_generated_press_pre( + generated_pre_posx, + label="generated PRESS_PRE above measured contact", + ) + for press_index in range(1, max(int(press_count), 1) + 1): + suffix = f" {press_index}/{press_count}" if press_count > 1 else "" + self.move_posx( + contact_posx, + label=f"Z-only descend to measured PRESS_CONTACT{suffix}", + velocity=self.args.press_line_velocity, + acceleration=self.args.press_line_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + if press_drop_m > 0.0: + pressed_posx = list(contact_posx) + pressed_posx[2] -= press_drop_m * 1000.0 + self.move_posx( + pressed_posx, + label=f"Z-only press from measured contact{suffix}", + 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), + ) + if self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + self.move_posx( + contact_posx, + label=f"return to measured PRESS_CONTACT{suffix}", + 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), + ) + elif self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + self.move_posx( + generated_pre_posx, + label=f"Z-only retreat to generated PRESS_PRE{suffix}", + velocity=self.args.press_line_velocity, + acceleration=self.args.press_line_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + if self.args.press_post_retreat_wait_seconds > 0.0: + time.sleep(self.args.press_post_retreat_wait_seconds) + return + + steps: list[tuple[list[float], str, float, float]] = [] + if joint_space_press: + if self.args.press_move_configured_prepose_before_joint: + self.move_posx( + [x_mm, y_mm, pre_z, rx, ry, rz], + label="fallback high pre pose before measured PRESS_CONTACT", + 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 fallback: no PRESS_PRE joints; using generated CONTACT+Z entry") + if self.args.press_contact_entry_lift_m > 0.0: + entry_z = contact_z + max(self.args.press_contact_entry_lift_m, 0.0) * 1000.0 + self.move_posx( + [x_mm, y_mm, entry_z, rx, ry, rz], + label="fallback safe high waypoint above measured PRESS_CONTACT", + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + contact_joints = self.normalize_joints_near_current( + contact_joints, + label="fallback PRESS_CONTACT measured joints", + ) + self.movej( + contact_joints, + label="fallback PRESS_CONTACT measured contact joints", + 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 + pressed_z = contact_z - press_drop_m * 1000.0 + print( + "[Azas] integrated press fallback: " + f"dispenser={dispenser_id} count={press_count} " + f"contact=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " + f"pressed_z={pressed_z:.1f} z_descent={contact_z - pressed_z:.1f}mm " + "source=live TCP after CONTACT-only fallback" + ) + 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 press_drop_m == 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 Z-only press start{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 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, + (transit_z if joint_space_press and not self.args.press_joint_space_use_high_prepose else 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,32 +3038,54 @@ 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)} " + # Legacy helper path only receives the taught contact pose here. The + # integrated path above is the normal hardware path for extra Z strokes. + "-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 " "-p pre_home_retreat_min_current_x_mm:=450.0 " - "-p pre_home_retreat_velocity:=20.0 " + "-p pre_home_retreat_velocity:=80.0 " "-p pre_home_retreat_acceleration:=25.0 " "-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 " @@ -516,11 +3094,11 @@ def press_cmd(args: argparse.Namespace, dispenser_id: str) -> str: "-p service_wait_timeout_sec:=10.0 " "-p pose_position_tolerance_mm:=8.0 " "-p pose_orientation_tolerance_deg:=6.0 " - "-p line_velocity:=20.0 " + "-p line_velocity:=80.0 " "-p line_acceleration:=30.0 " - "-p travel_line_velocity:=45.0 " + "-p travel_line_velocity:=80.0 " "-p travel_line_acceleration:=70.0 " - "-p joint_velocity:=40.0 " + "-p joint_velocity:=80.0 " "-p joint_acceleration:=50.0" ) @@ -582,40 +3160,743 @@ 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-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( + "--allow-tcp-set-failure", + action="store_true", + help=( + "Dangerous debug option: continue even if Doosan tcp/set_current_tcp fails. " + "Default false because measured press poses require the configured dispenser TCP." + ), + ) + parser.add_argument( + "--restore-tcp-after-run", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Restore the previous Doosan TCP when the sequence exits. Default false so " + "the measured gripper TCP stays active for follow-up recipe motions." + ), + ) + parser.add_argument("--move-velocity", type=float, default=80.0) + parser.add_argument("--move-acceleration", type=float, default=25.0) + parser.add_argument("--move-prehold-offset-x-m", type=float, default=-0.030) + parser.add_argument( + "--cup-pre-from-place-x-offset-m", + type=float, + default=-0.090, + help=( + "Generate DISP_PRE from the latest measured cup_place pose by changing only X. " + "Default -0.090m." + ), + ) + parser.add_argument( + "--cup-pre-from-place-z-offset-m", + type=float, + default=0.030, + help=( + "Generate DISP_PRE from the latest measured cup_place pose by changing only Z. " + "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, + default=False, + help=( + "Use calibration.yaml cup_common_pre_joints_deg before DISP_PLACE. " + "Default false: generate cup pre from cup_place with X/Z offsets." + ), + ) + parser.add_argument( + "--generated-cup-pre-use-joint-fallback", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Use IK MoveJoint for generated DISP_PRE. Default false: generated DISP_PRE " + "uses Cartesian MoveLine so X/Z-offset cup pre does not jump to a different wrist/joint branch." + ), + ) + parser.add_argument( + "--generated-cup-pre-max-joint-delta-deg", + type=float, + default=190.0, + help=( + "Joint delta guard used only when --generated-cup-pre-use-joint-fallback is enabled. " + "Default 190deg preserves the old fallback limit without affecting the default Cartesian path." + ), + ) + parser.add_argument( + "--move-prehold-offset-y-m", + type=float, + default=0.0, + help=( + "Y offset from measured front_hold for the pre-hold/above-hold approach. " + "The default is 0.0 because Y is the left/right slot axis, not the robot-side safety retreat." + ), + ) + parser.add_argument( + "--move-prehold-offset-z-m", + type=float, + default=0.180, + help="Vertical approach offset for initial cup placement at dispenser front-hold.", + ) + parser.add_argument( + "--move-release-offset-x-m", + type=float, + default=-0.020, + help=( + "Final cup release X offset from measured front_hold. Negative backs the cup " + "away from the dispenser toward the robot; default -0.020m keeps all cup-place " + "positions 20mm behind the taught front-hold." + ), + ) + parser.add_argument( + "--move-release-offset-y-m", + type=float, + default=0.0, + help="Final cup release Y offset from measured front_hold; default 0 avoids shifting dispenser 4 farther right.", + ) + parser.add_argument( + "--move-release-offset-z-m", + type=float, + default=0.0, + help="Final cup release Z offset from measured front_hold. Default 0 uses the taught cup-place height exactly.", + ) + parser.add_argument("--move-prehold-velocity", type=float, default=80.0) + parser.add_argument("--move-prehold-acceleration", type=float, default=22.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=80.0) + parser.add_argument("--pick-approach-acceleration", type=float, default=14.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-velocity", type=float, default=80.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=80.0) + parser.add_argument("--pick-lift-acceleration", type=float, default=25.0) parser.add_argument("--pick-timeout-sec", type=float, default=120.0) - parser.add_argument("--wait-service-sec", type=float, default=8.0) + parser.add_argument( + "--regrasp-min-transit-z-m", + type=float, + default=0.500, + help="Minimum absolute TCP Z for the vertical lift immediately after pressing, before returning to the cup.", + ) + parser.add_argument( + "--post-press-safe-lift-z-m", + type=float, + default=0.470, + help=( + "Initial vertical TCP Z after dispenser pressing before robot-side X retreat. " + "The following re-grasp high transit still uses --regrasp-min-transit-z-m." + ), + ) + parser.add_argument( + "--start-safe-lift-z-m", + type=float, + default=0.15, + help=( + "Minimum absolute TCP Z for the start-of-sequence vertical lift. The sequence " + "reads the live TCP pose first and raises only Z to this height before any " + "XY waypoint motion." + ), + ) + parser.add_argument( + "--min-allowed-tcp-z-m", + type=float, + default=0.02, + help=( + "Reject any Cartesian target pose whose absolute TCP Z is below this height. " + "A failed check prints an error and aborts before any robot command is sent." + ), + ) + parser.add_argument( + "--force-start-safe-lift", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "Always run the start-of-sequence Z-only safe lift regardless of the current " + "TCP height. With --no-force-start-safe-lift the lift still runs whenever the " + "live TCP Z is below --start-safe-lift-z-m." + ), + ) + parser.add_argument( + "--skip-release-pre", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Use the posture-independent safe release structure instead of the generated " + "DISP_PRE-from-DISP_PLACE release waypoint. Default false: cup pre is generated " + "from the latest cup_place pose by X offset." + ), + ) + parser.add_argument( + "--release-approach-lift-m", + type=float, + default=0.100, + help="Height of the generated release_above pose over the release final pose (same X/Y/orientation).", + ) + parser.add_argument( + "--release-start-safe-lift-m", + type=float, + default=0.120, + help="Relative Z-only lift from the live TCP pose at the start of the safe release.", + ) + parser.add_argument( + "--release-min-transit-z-m", + type=float, + default=0.300, + help=( + "Minimum absolute TCP Z for every XY-transit pose of the safe release " + "(start lift, staging, high entry). The vertical descent chain above->final " + "is exempt but still checked against --min-allowed-tcp-z-m." + ), + ) + parser.add_argument( + "--post-release-safe-lift-m", + type=float, + default=0.100, + help="Vertical lift above the release final pose right after the gripper opens.", + ) + parser.add_argument( + "--release-staging-x-m", + type=float, + default=None, + help="Optional absolute base-frame X of the release staging pose. Default: release final X.", + ) + parser.add_argument( + "--release-staging-y-m", + type=float, + default=None, + help="Optional absolute base-frame Y of the release staging pose. Default: release final Y.", + ) + parser.add_argument( + "--release-staging-z-m", + type=float, + default=None, + help="Optional absolute base-frame Z of the release staging pose. Default: release transit height.", + ) + parser.add_argument( + "--use-release-staging", + action=argparse.BooleanOptionalAction, + default=True, + help="Route the safe release through the staging pose before the above-final entry.", + ) + parser.add_argument( + "--regrasp-approach-offset-z-m", + type=float, + default=0.250, + help=( + "High front-hold Z offset used for the post-press re-grasp transit after the gripper " + "has opened at the lifted robot-side retreat pose." + ), + ) + parser.add_argument( + "--regrasp-max-transit-z-m", + type=float, + 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=80.0) + parser.add_argument("--regrasp-approach-acceleration", type=float, default=18.0) + parser.add_argument( + "--regrasp-reset-before-cup", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "After pressing, move through a fixed HOME joint waypoint before returning to the cup. " + "Default true because the Cartesian post-press high-Z lift can stall near the dispenser." + ), + ) + parser.add_argument( + "--regrasp-reset-joints-deg", + default="0,0,90,0,90,180", + help="HOME joint pose used after dispenser press, before returning to cup re-grasp.", + ) + parser.add_argument("--regrasp-reset-joint-velocity", type=float, default=80.0) + parser.add_argument("--regrasp-reset-joint-acceleration", type=float, default=35.0) + parser.add_argument( + "--regrasp-retreat-x-m", + type=float, + default=-0.080, + help=( + "Optional high-Z X retreat immediately after press before returning to cup. " + "In the measured dispenser setup, negative X backs away from the dispenser toward the robot." + ), + ) + parser.add_argument( + "--regrasp-retreat-y-m", + type=float, + default=0.0, + help="Optional high-Z Y shift immediately after press before returning to cup; not used as the default safety retreat.", + ) + parser.add_argument( + "--regrasp-rear-entry-offset-x-m", + type=float, + default=-0.090, + help=( + "Extra X offset from final release pose for the lowered rear entry before re-grasp. " + "Negative X approaches from the robot side instead of sliding sideways along dispenser slots." + ), + ) + parser.add_argument( + "--regrasp-rear-entry-offset-y-m", + type=float, + default=0.0, + help="Extra Y offset from final release pose for the lowered rear entry before moving forward to the 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.070, + help=( + "Z-only pump stroke after the measured PRESS_CONTACT joints touch the dispenser. " + "Default is 0.070 m (7 cm)." + ), + ) + parser.add_argument( + "--press-extra-depth-m", + type=float, + default=0.0, + help=( + "Additional Z-only pump stroke added after PRESS_CONTACT. " + "Total stroke is --press-depth-m + this value. Default 0." + ), + ) + parser.add_argument( + "--press-pre-lift-m", + type=float, + default=0.080, + help="Cartesian fallback lift above the measured dispenser contact pose.", + ) + parser.add_argument("--press-approach-height-m", type=float, default=0.100) + 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.050, + help="X retreat after cup release and before the vertical press lift; negative backs toward the robot.", + ) + parser.add_argument( + "--press-pre-lift-retreat-y-m", + type=float, + default=0.0, + help="Y shift after cup release and before the vertical press lift; not used as the default dispenser retreat.", + ) + parser.add_argument( + "--press-min-transit-z-m", + type=float, + 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=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) + parser.add_argument("--press-gripper-force-n", type=float, default=30.0) + parser.add_argument( + "--press-reset-before-press", + action=argparse.BooleanOptionalAction, + default=True, + 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. " + "Default false because the measured common pre/HOME joint waypoint can choose a large wrist branch." + ), + ) + parser.add_argument( + "--press-reset-joints-deg", + default="0,0,90,0,90,0", + help="Fallback HOME joint pose used only when press_common_pre_joints_deg is missing.", + ) + parser.add_argument("--press-reset-joint-velocity", type=float, default=80.0) + 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=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, + 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, + default=0.050, + help=( + "Generated Cartesian CONTACT_ENTRY_LIFT height above measured PRESS_CONTACT FK. " + "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, + default=True, + help=( + "Default true: ignore calibration.yaml press_pre_joints_deg and approach/retreat " + "through PRESS_CONTACT FK with Z raised by --press-contact-entry-lift-m. " + "Use --no-skip-measured-press-pre to restore the old measured PRESS_PRE joint path." + ), + ) + parser.add_argument( + "--press-joint-space-use-high-prepose", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Fallback-only option for old contact-joint mode. Default false; measured " + "PRESS_PRE/PRESS_CONTACT pairs do not need this Cartesian pre_z." + ), + ) + 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( + "--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, + 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=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( + "--safe-lift-target-tolerance-mm", + type=float, + default=30.0, + help=( + "XYZ tolerance for high safe-lift verification. The Doosan controller can stop " + "roughly 20mm from the requested high Z while still clearing the dispenser/cup; " + "keep front-hold/press targets on --target-tolerance-mm." + ), + ) + parser.add_argument( + "--post-press-safe-lift-target-tolerance-mm", + type=float, + default=60.0, + help=( + "XYZ tolerance only for the vertical lift immediately after dispenser pressing. " + "This avoids stalling on the lifted clearance pose while keeping other safe-lift " + "and re-grasp checks at --safe-lift-target-tolerance-mm." + ), + ) + 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( + "--normalize-measured-joints-near-current", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Compatibility mode for old measured-joint execution. Default false: " + "operator-measured DISP/PRESS joints are commanded exactly, without " + "rewriting them to a nearby 360deg-equivalent branch." + ), + ) + 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, + 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=80.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=80.0) + parser.add_argument("--safe-lift-joint-fallback-acceleration", type=float, default=40.0) + parser.add_argument( + "--press-generated-pre-use-joint", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "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=False, + help=( + "When MoveLine to generated PRESS_PRE returns complete but target verification stalls, " + "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") 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( + "--final-regrasp-extra-x-offset-m", + type=float, + 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." + ), + ) + parser.add_argument("--final-regrasp-extra-y-offset-m", type=float, default=0.0) + parser.add_argument("--final-regrasp-extra-z-offset-m", type=float, default=0.0) + parser.add_argument("--final-regrasp-grasp-width-m", type=float, default=0.068) + parser.add_argument("--final-regrasp-force-n", type=float, default=35.0) + parser.add_argument( + "--place-cup-holder-after-sequence", + action=argparse.BooleanOptionalAction, + 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.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", + 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) + parser.add_argument("--cup-holder-place-acceleration", type=float, default=10.0) + parser.add_argument("--cup-holder-retreat-velocity", type=float, default=80.0) + parser.add_argument("--cup-holder-retreat-acceleration", type=float, default=16.0) + parser.add_argument("--cup-holder-timeout-sec", type=float, default=90.0) + parser.add_argument("--cup-holder-target-tolerance-mm", type=float, default=12.0) + parser.add_argument("--cup-holder-x-min-m", type=float, default=0.35) + 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.06) + parser.add_argument("--cup-holder-z-max-m", type=float, default=0.28) + parser.add_argument( + "--gripper-settle-seconds", + type=float, + 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=1.5, + 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( + "--press-lock-contact-joints", + default="", + help=( + "Comma-separated joint numbers copied from press_pre_joints_deg into " + "press_contact_joints_deg before MoveJoint. Default empty: command measured PRESS_CONTACT exactly." + ), + ) + parser.add_argument( + "--press-use-recorded-pre-joints", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "Deprecated compatibility flag accepted for old commands. Ignored by the integrated path: " + "use --no-skip-measured-press-pre when the old measured PRESS_PRE joint path is explicitly needed." + ), + ) + parser.add_argument( + "--press-contact-joint-pose-tolerance-mm", + type=float, + default=35.0, + help=( + "Compare measured press_contact_joints_deg FK against press_pose_xyz_m and warn " + "when the distance exceeds this tolerance. Set <=0 to disable the check." + ), + ) + parser.add_argument( + "--strict-press-contact-joint-pose-match", + action="store_true", + help=( + "Fail if press_contact_joints_deg FK and press_pose_xyz_m differ beyond " + "--press-contact-joint-pose-tolerance-mm. Default false because this controller's " + "Fkin can report a different TCP basis than the taught press pose." + ), + ) + parser.add_argument( + "--allow-press-contact-joint-pose-mismatch", + action="store_true", + help="Deprecated compatibility flag for overriding strict FK/press_pose mismatch failures.", + ) 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=False, + help=( + "If the persistent integrated re-grasp/lift stalls verification, retry once " + "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( + "--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( + "--resume", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "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( + "--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") - 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", + ) + args.regrasp_reset_joints_deg = parse_float_list( + args.regrasp_reset_joints_deg, + expected_count=6, + label="--regrasp-reset-joints-deg", + ) + args.press_lock_contact_joint_indexes = parse_joint_index_set( + args.press_lock_contact_joints, + label="--press-lock-contact-joints", + ) + return args def main() -> int: @@ -639,79 +3920,316 @@ 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) + ) 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(f"[Azas] press_lock_contact_joints={args.press_lock_contact_joints or '-'}") + print( + f"[Azas] measured_joint_press_z_overdrive_m={max(args.press_depth_m, 0.0) + max(args.press_extra_depth_m, 0.0):.3f} " + f"(press_depth_m={args.press_depth_m:.3f}, extra={args.press_extra_depth_m:.3f})" + ) + print(f"[Azas] skip_measured_press_pre={str(args.skip_measured_press_pre).lower()}") + if args.skip_measured_press_pre: + print( + "[Azas] source=calibration.yaml generated DISP_PRE from DISP_PLACE X/Z offset, measured DISP_PLACE, " + "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") + release_path = ( + "SAFE_RELEASE(Z_LIFT -> STAGING/ABOVE -> vertical RELEASE_FINAL -> OPEN -> POST_LIFT)" + if args.skip_release_pre + else ( + "CUP_COMMON_PRE -> DISP_PLACE -> RELEASE" + if args.use_cup_common_pre + else "GENERATED_DISP_PRE(X/Z offset) -> DISP_PLACE -> RELEASE" + ) + ) + press_common_pre_path = "PRESS_COMMON_PRE -> " if args.press_reset_before_press else "" + press_path = ( + f"SAFE_LIFT -> {press_common_pre_path}CONTACT_ENTRY_LIFT -> PRESS_CONTACT -> PRESS_Z_OVERDRIVE -> " + "PRESS_CONTACT -> POST_PRESS_SAFE_LIFT -> REGRASP" + if args.skip_measured_press_pre + else ( + "SAFE_LIFT -> PRESS_PRE -> CONTACT_ENTRY_LIFT -> PRESS_CONTACT -> " + "PRESS_Z_OVERDRIVE -> PRESS_CONTACT -> PRESS_PRE -> REGRASP" + ) + ) + print(f"[Azas] default_path=START_SAFE_LIFT -> {release_path} -> {press_path}") + print( + f"[Azas] start_safety: force_start_safe_lift={args.force_start_safe_lift} " + f"start_safe_lift_z_m={max(args.start_safe_lift_z_m, 0.0):.3f} " + f"min_allowed_tcp_z_m={max(args.min_allowed_tcp_z_m, 0.0):.3f}" + ) + print( + f"[Azas] release_safety: skip_release_pre={args.skip_release_pre} " + f"use_release_staging={args.use_release_staging} " + f"release_start_safe_lift_m={max(args.release_start_safe_lift_m, 0.0):.3f} " + f"release_min_transit_z_m={max(args.release_min_transit_z_m, 0.0):.3f} " + f"release_approach_lift_m={max(args.release_approach_lift_m, 0.0):.3f} " + f"post_release_safe_lift_m={max(args.post_release_safe_lift_m, 0.0):.3f}" + ) + 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}" + ) + 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: 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}") + if motion is not None: + motion.close() + return 1 + try: + motion.ensure_start_safe_pose() + except RuntimeError as exc: + print(f"[FAIL] start safe lift failed; no recipe motion sent: {exc}") + motion.close() + return 1 elif args.execute: print("[Azas] integrated_motion=false (legacy subprocess primitives requested)") + if args.force_start_safe_lift or args.skip_release_pre: + print( + "[BLOCKED] legacy subprocess primitives cannot run the start safe lift or the " + "posture-independent safe release; use integrated motion or pass " + "--no-force-start-safe-lift and --no-skip-release-pre explicitly" + ) + return 2 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}" + 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) + except ValueError as exc: + print(f"[BLOCKED] {label_prefix}: {exc}") + return 1 if not args.execute: - print(f"[PLAN] {label_prefix}: integrated move/release -> press -> integrated re-grasp/lift (move/release -> press -> re-grasp/lift)") + move_release_step = "skip initial move/release" if args.skip_initial_move_release else "integrated move/release" + print( + f"[PLAN] {label_prefix}: {move_release_step} -> " + f"integrated press {press_count} time(s) -> integrated re-grasp/lift" + ) + print_dry_run_group_detail(args, dispenser_id, press_count) continue - if motion is None: + 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" + ) + 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: return rc 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") - 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 and (press_needed or regrasp_needed): + 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 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)", + press_cmd(args, dispenser_id, press_count), + ) + if rc != 0: + return rc + 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 + 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}") + motion.regrasp_and_lift(dispenser_id, final_regrasp=final_regrasp) + except RuntimeError as exc: + 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 + resume_tracker.complete_stage(index, "regrasp") + else: + print(f"[Azas] SKIP {label_prefix}: re-grasp/lift completed in resume_state") + + if motion is None and regrasp_needed: + 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 not args.execute: + if args.place_cup_holder_after_sequence: + print( + "[PLAN] after final re-grasp: CUP_HOLDER_PRE_PLACE -> " + "CUP_HOLDER_PLACE_FINAL -> RG2_OPEN -> CUP_HOLDER_RETREAT" + ) + elif args.place_cup_holder_after_sequence: + 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", + [ + sys.executable, + str(ROOT / "tools" / "run" / "place_side_grip_cup_in_holder.py"), + "--service-prefix", + args.service_prefix, + "--config", + str(CALIBRATION_CONFIG), + "--motion-backend", + "direct", + "--approach-velocity", + f"{args.cup_holder_approach_velocity:.6f}", + "--approach-acceleration", + f"{args.cup_holder_approach_acceleration:.6f}", + "--place-velocity", + f"{args.cup_holder_place_velocity:.6f}", + "--place-acceleration", + f"{args.cup_holder_place_acceleration:.6f}", + "--retreat-velocity", + f"{args.cup_holder_retreat_velocity:.6f}", + "--retreat-acceleration", + 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", + f"{args.cup_holder_rz_offset_deg:.6f}", + "--timeout-sec", + f"{args.cup_holder_timeout_sec:.6f}", + "--target-tolerance-mm", + f"{args.cup_holder_target_tolerance_mm:.6f}", + "--execute", + "--confirm", + "ENABLE_CUP_HOLDER_PLACE", + ], + ) + if rc != 0: + return rc + resume_tracker.complete_all() + elif cup_holder_needed: try: - motion.regrasp_and_lift(dispenser_id) + print("[Azas] final: PLACE CUP IN HOLDER") + motion.place_cup_in_holder() except RuntimeError as exc: - print(f"[FAIL] {label_prefix}: integrated re-grasp/lift failed: {exc}") + print(f"[FAIL] final cup-holder place 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 + 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_minimal_dispenser_cycle.py b/tools/run/run_minimal_dispenser_cycle.py new file mode 100755 index 0000000..5fef757 --- /dev/null +++ b/tools/run/run_minimal_dispenser_cycle.py @@ -0,0 +1,847 @@ +#!/usr/bin/env python3 +"""Minimal measured dispenser cycle without MoveIt planning guards. + +Sequence per dispenser group: + measured cup pre-place -> measured cup place/open -> measured cup pre-place -> + measured press pre -> measured press/contact -> lift/back/open -> + measured cup pre-place -> measured cup place/grasp -> lift. + +All target values are loaded from calibration.yaml. This script does not ask +for, invent, or persist new robot coordinates. +""" + +from __future__ import annotations + +import argparse +import math +import sys +import time +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import rclpy +import yaml +from azas_interfaces.srv import SetGripper +from dsr_msgs2.srv import GetCurrentPosx, MoveJoint, MoveLine, MoveWait + + +ROOT = Path("/home/ssu/Azas") +DEFAULT_CALIBRATION = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +CONFIRM_PHRASE = "ENABLE_MINIMAL_DISPENSER_CYCLE" + +DR_BASE = 0 +MOVE_MODE_ABSOLUTE = 0 +SYNC = 0 +BLENDING_SPEED_TYPE_DUPLICATE = 0 +INVALID_PRESS_CONTACT_STATUSES = { + "invalid", + "invalid_reteach_required", + "needs_reteach", + "reteach_required", + "확인 필요", +} + + +@dataclass(frozen=True) +class DispenserCalibration: + dispenser_id: str + cup_pre_place_joints_deg: list[float] | None + cup_place_joints_deg: list[float] | None + outlet_xyz_m: list[float] + outlet_zyz_deg: list[float] + press_xyz_m: list[float] + press_zyz_deg: list[float] + press_pre_joints_deg: list[float] | None + press_contact_joints_deg: list[float] + + +def numeric_list(value: Any, label: str, count: int) -> list[float]: + if not isinstance(value, list) or len(value) != count: + raise ValueError(f"{label} must be a {count}-number list") + try: + return [float(item) for item in value] + except (TypeError, ValueError) as exc: + raise ValueError(f"{label} must contain only numbers") from exc + + +def optional_numeric_list(value: Any, label: str, count: int) -> list[float] | None: + if value is None: + return None + return numeric_list(value, label, count) + + +def require_cup_place_joints(cfg: DispenserCalibration) -> tuple[list[float], list[float]]: + if cfg.cup_pre_place_joints_deg is None or cfg.cup_place_joints_deg is None: + raise ValueError( + f"dispenser_outlets.{cfg.dispenser_id} must define " + "cup_pre_place_joints_deg and cup_place_joints_deg for joint cup-place mode" + ) + return cfg.cup_pre_place_joints_deg, cfg.cup_place_joints_deg + + +def require_press_joints(cfg: DispenserCalibration) -> tuple[list[float], list[float]]: + if cfg.press_pre_joints_deg is None: + raise ValueError(f"dispenser_outlets.{cfg.dispenser_id} must define press_pre_joints_deg for joint press mode") + return cfg.press_pre_joints_deg, cfg.press_contact_joints_deg + + +def unwrap_joint_target_near(reference: list[float], target: list[float]) -> list[float]: + adjusted: list[float] = [] + for ref_value, target_value in zip(reference, target): + candidates = [target_value - 360.0, target_value, target_value + 360.0] + adjusted.append(min(candidates, key=lambda candidate: abs(candidate - ref_value))) + return adjusted + + +def parse_joint_index_set(raw: str) -> set[int]: + result: set[int] = set() + for item in raw.replace(";", ",").split(","): + value = item.strip().lower() + if not value: + continue + if value.startswith("j"): + value = value[1:] + index = int(value) + if not 1 <= index <= 6: + raise ValueError(f"joint index out of range: {item!r}") + result.add(index - 1) + return result + + +def lock_joints_to_reference(target: list[float], reference: list[float], joint_indexes: set[int]) -> list[float]: + adjusted = list(target) + for index in joint_indexes: + adjusted[index] = reference[index] + return adjusted + + +def quaternion_to_matrix_xyzw(quaternion: list[float]) -> list[list[float]]: + x, y, z, w = quaternion + norm = math.sqrt(x * x + y * y + z * z + w * w) + if norm <= 0.0: + raise ValueError("quaternion norm must be non-zero") + x, y, z, w = x / norm, y / norm, z / norm, w / norm + return [ + [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)], + [2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)], + [2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)], + ] + + +def matrix_to_doosan_zyz_deg(matrix: list[list[float]]) -> list[float]: + beta = math.acos(max(-1.0, min(1.0, matrix[2][2]))) + sin_beta = math.sin(beta) + if abs(sin_beta) > 1e-8: + alpha = math.atan2(matrix[1][2], matrix[0][2]) + gamma = math.atan2(matrix[2][1], -matrix[2][0]) + else: + alpha = 0.0 + gamma = math.atan2(-matrix[0][1], matrix[0][0]) + return [math.degrees(value) for value in (alpha, beta, gamma)] + + +def quaternion_to_doosan_zyz_deg(quaternion: list[float]) -> list[float]: + return matrix_to_doosan_zyz_deg(quaternion_to_matrix_xyzw(quaternion)) + + +def parse_dispenser_ids(raw: str) -> list[str]: + 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}") + count = int(count_raw.strip()) + if count < 1: + raise ValueError(f"count must be >= 1 for dispenser {dispenser_id}") + result.extend([dispenser_id] * count) + if not result: + raise ValueError("at least one dispenser id is required") + return result + + +def parse_dispenser_id_set(raw: str) -> set[str]: + result: set[str] = set() + for item in raw.replace(";", ",").split(","): + value = item.strip() + if not value: + continue + if value not in {"1", "2", "3", "4"}: + raise ValueError(f"unsupported waypoint dispenser id: {value!r}") + result.add(value) + return result + + +def group_consecutive(values: list[str]) -> list[tuple[str, int]]: + groups: list[tuple[str, int]] = [] + for value in values: + if groups and groups[-1][0] == value: + groups[-1] = (value, groups[-1][1] + 1) + else: + groups.append((value, 1)) + return groups + + +def load_dispenser(calibration_path: Path, dispenser_id: str) -> DispenserCalibration: + data = yaml.safe_load(calibration_path.read_text(encoding="utf-8")) or {} + block = (data.get("dispenser_outlets") or {}).get(dispenser_id) + if not isinstance(block, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} missing in {calibration_path}") + status = str(block.get("press_contact_status", "")).strip() + if status.lower() in INVALID_PRESS_CONTACT_STATUSES: + raise ValueError( + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg is marked " + f"{status!r}; refusing dispenser cycle until PRESS{dispenser_id}_CONTACT is re-taught" + ) + outlet_xyz_m = numeric_list(block.get("outlet_pose_xyz_m"), f"outlet {dispenser_id} outlet_pose_xyz_m", 3) + outlet_q = numeric_list(block.get("outlet_pose_quaternion_xyzw"), f"outlet {dispenser_id} outlet_pose_quaternion_xyzw", 4) + press_xyz_m = numeric_list(block.get("press_pose_xyz_m"), f"outlet {dispenser_id} press_pose_xyz_m", 3) + press_q = numeric_list(block.get("press_pose_quaternion_xyzw"), f"outlet {dispenser_id} press_pose_quaternion_xyzw", 4) + press_pre_joints = optional_numeric_list(block.get("press_pre_joints_deg"), f"outlet {dispenser_id} press_pre_joints_deg", 6) + press_joints = numeric_list(block.get("press_contact_joints_deg"), f"outlet {dispenser_id} press_contact_joints_deg", 6) + cup_pre_place_joints = optional_numeric_list( + block.get("cup_pre_place_joints_deg"), + f"outlet {dispenser_id} cup_pre_place_joints_deg", + 6, + ) + cup_place_joints = optional_numeric_list( + block.get("cup_place_joints_deg"), + f"outlet {dispenser_id} cup_place_joints_deg", + 6, + ) + return DispenserCalibration( + dispenser_id=dispenser_id, + cup_pre_place_joints_deg=cup_pre_place_joints, + cup_place_joints_deg=cup_place_joints, + outlet_xyz_m=outlet_xyz_m, + outlet_zyz_deg=quaternion_to_doosan_zyz_deg(outlet_q), + press_xyz_m=press_xyz_m, + press_zyz_deg=quaternion_to_doosan_zyz_deg(press_q), + press_pre_joints_deg=press_pre_joints, + press_contact_joints_deg=press_joints, + ) + + +def load_press_transfer_waypoint(calibration_path: Path, waypoint_name: str) -> list[float] | None: + if waypoint_name == "none": + return None + data = yaml.safe_load(calibration_path.read_text(encoding="utf-8")) or {} + if waypoint_name == "color_scan": + return numeric_list(data.get("color_scan_pose", {}).get("joints_deg"), "color_scan_pose.joints_deg", 6) + raise ValueError(f"unsupported press transfer waypoint: {waypoint_name}") + + +def service_name(prefix: str, suffix: str) -> str: + clean = prefix.strip("/") + return f"/{clean}/{suffix}" if clean else f"/{suffix}" + + +class MinimalCycle: + def __init__(self, args: argparse.Namespace) -> None: + self.args = args + rclpy.init(args=None) + self.node = rclpy.create_node("azas_minimal_dispenser_cycle") + 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.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) + self.press_transfer_waypoint_joints = load_press_transfer_waypoint(args.calibration, args.press_transfer_waypoint) + self.press_waypoint_dispenser_ids = parse_dispenser_id_set(args.press_waypoint_dispenser_ids) + self.press_lock_joint_indexes = parse_joint_index_set(args.press_lock_joints) + self.last_joint_target_deg: list[float] | None = None + + def close(self) -> None: + self.node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + def preflight(self) -> None: + required = [ + (self.move_line, "MoveLine"), + (self.move_joint, "MoveJoint"), + (self.move_wait, "MoveWait"), + (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: " + ", ".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', '')}") + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=max(timeout_sec, 0.1)) + if not future.done(): + raise RuntimeError(f"{label} response timeout after {timeout_sec:.1f}s") + if future.exception() is not None: + raise RuntimeError(f"{label} exception: {future.exception()}") + response = future.result() + if response is None: + raise RuntimeError(f"{label} returned no response") + return response + + def wait_motion(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 bool(getattr(response, "success", True)): + raise RuntimeError(f"MoveWait returned success=false for {label}") + + def current_posx(self) -> list[float]: + req = GetCurrentPosx.Request() + req.ref = DR_BASE + response = self._call(self.get_posx, req, timeout_sec=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]] + + def validate_xyz(self, pos: list[float], label: str) -> None: + x, y, z = pos[:3] + failures = [] + if not self.args.x_min <= x / 1000.0 <= self.args.x_max: + failures.append(f"x={x / 1000.0:.3f} outside [{self.args.x_min:.3f}, {self.args.x_max:.3f}]") + if not self.args.y_min <= y / 1000.0 <= self.args.y_max: + failures.append(f"y={y / 1000.0:.3f} outside [{self.args.y_min:.3f}, {self.args.y_max:.3f}]") + if not self.args.z_min <= z / 1000.0 <= self.args.z_max: + failures.append(f"z={z / 1000.0:.3f} outside [{self.args.z_min:.3f}, {self.args.z_max:.3f}]") + if failures: + raise RuntimeError(f"{label} outside direct MoveLine bounds: " + "; ".join(failures)) + + def validate_joints(self, joints_deg: list[float], label: str) -> None: + failures = [] + for index, value in enumerate(joints_deg, start=1): + if not self.args.joint_min_deg <= value <= self.args.joint_max_deg: + failures.append( + f"j{index}={value:.2f} outside " + f"[{self.args.joint_min_deg:.0f}, {self.args.joint_max_deg:.0f}]" + ) + if not self.args.j5_min_deg <= joints_deg[4] <= self.args.j5_max_deg: + failures.append( + f"j5={joints_deg[4]:.2f} outside " + f"[{self.args.j5_min_deg:.0f}, {self.args.j5_max_deg:.0f}]" + ) + if failures: + raise RuntimeError(f"{label} outside direct MoveJoint bounds: " + "; ".join(failures)) + + def movel(self, pos: list[float], label: str, velocity: float, acceleration: float) -> None: + self.validate_xyz(pos, label) + print( + f"[Azas] {label}: movel posx=[{pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}, " + f"{pos[3]:.1f}, {pos[4]:.1f}, {pos[5]:.1f}] vel={velocity:.1f} acc={acceleration:.1f}" + ) + req = MoveLine.Request() + req.pos = [float(value) for value in pos] + req.vel = [float(velocity), float(velocity)] + req.acc = [float(acceleration), float(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=self.args.motion_timeout_sec, label=f"MoveLine {label}") + if not response.success: + raise RuntimeError(f"MoveLine returned success=false for {label}") + self.wait_motion(label, self.args.motion_timeout_sec) + + def unwrap_near_last_joint_target(self, joints_deg: list[float], label: str) -> list[float]: + if self.last_joint_target_deg is None or self.args.disable_joint_branch_unwrap: + return list(joints_deg) + adjusted = unwrap_joint_target_near(self.last_joint_target_deg, joints_deg) + if any(abs(raw - command) > 1e-6 for raw, command in zip(joints_deg, adjusted)): + print( + "[Azas] " + + label + + ": unwrap joint branch raw=[" + + ", ".join(f"{value:.1f}" for value in joints_deg) + + "] command=[" + + ", ".join(f"{value:.1f}" for value in adjusted) + + "]" + ) + return adjusted + + def movej( + self, + joints_deg: list[float], + label: str, + velocity: float, + acceleration: float, + ) -> None: + command_joints = self.unwrap_near_last_joint_target(joints_deg, label) + self.validate_joints(command_joints, label) + print( + "[Azas] " + + label + + ": movej_deg=[" + + ", ".join(f"{value:.1f}" for value in command_joints) + + f"] vel={velocity:.1f} acc={acceleration:.1f}" + ) + req = MoveJoint.Request() + req.pos = [float(value) for value in command_joints] + 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.motion_timeout_sec, label=f"MoveJoint {label}") + if not response.success: + raise RuntimeError(f"MoveJoint returned success=false for {label}") + self.wait_motion(label, self.args.motion_timeout_sec) + self.last_joint_target_deg = command_joints + + def should_use_press_waypoint(self, cfg: DispenserCalibration) -> bool: + return self.press_transfer_waypoint_joints is not None and cfg.dispenser_id in self.press_waypoint_dispenser_ids + + def move_press_transfer_waypoint(self, cfg: DispenserCalibration, label: str) -> bool: + if not self.should_use_press_waypoint(cfg): + return False + assert self.press_transfer_waypoint_joints is not None + self.movej( + self.press_transfer_waypoint_joints, + label, + self.args.press_waypoint_velocity, + self.args.press_waypoint_acceleration, + ) + return True + + def gripper_command(self, command: str, width_m: float, force_n: float, label: str) -> None: + print(f"[Azas] {label}: gripper command={command} width_m={width_m:.3f} force_n={force_n:.1f}") + req = SetGripper.Request() + req.command = command + req.width_m = float(width_m) + req.force_n = float(force_n) + response = self._call(self.gripper, req, timeout_sec=self.args.gripper_timeout_sec, label=label) + if not response.success: + raise RuntimeError(f"{label} returned success=false: {response.message}") + print(f"[Azas] {label}: {response.message}") + settle = self.args.gripper_open_settle_sec if command == "open" else self.args.gripper_settle_sec + if settle > 0.0: + time.sleep(settle) + + def lift_current_to(self, min_z_m: float, label: str, velocity: float, acceleration: float) -> None: + pose = self.current_posx() + target_z_mm = max(float(pose[2]), max(min_z_m, 0.0) * 1000.0) + if target_z_mm <= pose[2] + 1.0: + print(f"[Azas] {label}: already high enough z={pose[2] / 1000.0:.3f}m") + return + target = [pose[0], pose[1], target_z_mm, pose[3], pose[4], pose[5]] + self.movel(target, label, velocity, acceleration) + + def shift_current_x(self, dx_m: float, label: str, velocity: float, acceleration: float) -> None: + if abs(dx_m) < 1e-6: + print(f"[Azas] {label}: skipped dx=0") + return + pose = self.current_posx() + target = [pose[0] + dx_m * 1000.0, pose[1], pose[2], pose[3], pose[4], pose[5]] + self.movel(target, label, velocity, acceleration) + + @staticmethod + def posx(xyz_m: list[float], zyz_deg: list[float]) -> list[float]: + return [xyz_m[0] * 1000.0, xyz_m[1] * 1000.0, xyz_m[2] * 1000.0, *zyz_deg] + + def run_group(self, cfg: DispenserCalibration, press_count: int) -> None: + place = self.posx(cfg.outlet_xyz_m, cfg.outlet_zyz_deg) + pre_place_xyz = [ + cfg.outlet_xyz_m[0] + self.args.cup_pre_x_m, + cfg.outlet_xyz_m[1] + self.args.cup_pre_y_m, + cfg.outlet_xyz_m[2] + self.args.cup_pre_z_m, + ] + pre_place = self.posx(pre_place_xyz, cfg.outlet_zyz_deg) + press_pre_xyz = [ + cfg.press_xyz_m[0], + cfg.press_xyz_m[1], + cfg.press_xyz_m[2] + self.args.press_pre_z_m, + ] + press_pre = self.posx(press_pre_xyz, cfg.press_zyz_deg) + + print(f"[Azas] === dispenser {cfg.dispenser_id} x{press_count} minimal cycle ===") + if self.args.cup_place_mode == "joint": + cup_pre_place_joints, cup_place_joints = require_cup_place_joints(cfg) + self.movej( + cup_pre_place_joints, + "cup pre-place measured joints", + self.args.travel_velocity, + self.args.travel_acceleration, + ) + self.movej( + cup_place_joints, + "cup place measured joints", + self.args.approach_velocity, + self.args.approach_acceleration, + ) + else: + self.movel(pre_place, "cup pre-place robot-side/up", self.args.travel_velocity, self.args.travel_acceleration) + self.movel(place, "cup place at measured outlet", self.args.approach_velocity, self.args.approach_acceleration) + self.gripper_command("open", self.args.gripper_open_width_m, self.args.gripper_open_force_n, "release cup") + + if self.args.cup_place_mode == "joint": + cup_pre_place_joints, _ = require_cup_place_joints(cfg) + self.movej( + cup_pre_place_joints, + "exit cup place to measured pre-place", + self.args.travel_velocity, + self.args.travel_acceleration, + ) + else: + self.lift_current_to(self.args.after_release_min_z_m, "lift after cup release", self.args.travel_velocity, self.args.travel_acceleration) + self.shift_current_x(self.args.after_release_retreat_x_m, "robot-side retreat after release", self.args.travel_velocity, self.args.travel_acceleration) + self.gripper_command("set_width", self.args.press_gripper_width_m, self.args.press_gripper_force_n, "close empty gripper before press") + + if self.args.press_mode == "joint": + press_pre_joints, press_contact_joints = require_press_joints(cfg) + press_contact_command_joints = lock_joints_to_reference( + press_contact_joints, + press_pre_joints, + self.press_lock_joint_indexes, + ) + if not self.args.disable_press_entry_waypoint: + self.move_press_transfer_waypoint(cfg, f"press entry transfer waypoint before dispenser {cfg.dispenser_id}") + self.movej( + press_pre_joints, + "press pre measured joints", + self.args.travel_velocity, + self.args.travel_acceleration, + ) + for index in range(1, press_count + 1): + suffix = f" {index}/{press_count}" if press_count > 1 else "" + self.movej( + press_contact_command_joints, + f"press contact measured joints{suffix}", + self.args.press_joint_velocity, + self.args.press_joint_acceleration, + ) + if self.args.press_hold_sec > 0.0: + time.sleep(self.args.press_hold_sec) + self.movej( + press_pre_joints, + f"press release to measured pre{suffix}", + self.args.press_joint_velocity, + self.args.press_joint_acceleration, + ) + else: + self.movel(press_pre, "press pre-pose above dispenser", self.args.travel_velocity, self.args.travel_acceleration) + self.movej(cfg.press_contact_joints_deg, "press contact measured joints", self.args.press_joint_velocity, self.args.press_joint_acceleration) + + contact = self.current_posx() + up = list(contact) + down = list(contact) + down[2] = contact[2] - max(self.args.press_depth_m, 0.0) * 1000.0 + for index in range(1, press_count + 1): + suffix = f" {index}/{press_count}" if press_count > 1 else "" + if self.args.press_depth_m > 0.0: + self.movel(down, f"press pump down{suffix}", self.args.press_line_velocity, self.args.press_line_acceleration) + self.movel(up, f"press pump release/up{suffix}", self.args.press_line_velocity, self.args.press_line_acceleration) + if self.args.press_hold_sec > 0.0: + time.sleep(self.args.press_hold_sec) + + exited_via_waypoint = False + if self.args.press_mode == "joint" and not self.args.disable_press_exit_waypoint: + exited_via_waypoint = self.move_press_transfer_waypoint( + cfg, + f"press exit transfer waypoint after dispenser {cfg.dispenser_id}", + ) + if not exited_via_waypoint: + self.lift_current_to(self.args.post_press_min_z_m, "lift after press before re-grasp", self.args.travel_velocity, self.args.travel_acceleration) + self.shift_current_x(self.args.post_press_retreat_x_m, "robot-side retreat after press", self.args.travel_velocity, self.args.travel_acceleration) + if self.args.cup_place_mode == "joint": + cup_pre_place_joints, cup_place_joints = require_cup_place_joints(cfg) + self.movej( + cup_pre_place_joints, + "re-grasp pre-place measured joints", + self.args.travel_velocity, + self.args.travel_acceleration, + ) + self.gripper_command( + "open", + self.args.gripper_open_width_m, + self.args.gripper_open_force_n, + "open gripper at cup pre-place before re-grasp", + ) + self.movej( + cup_place_joints, + "re-grasp cup place measured joints", + self.args.approach_velocity, + self.args.approach_acceleration, + ) + else: + self.gripper_command( + "open", + self.args.gripper_open_width_m, + self.args.gripper_open_force_n, + "open gripper away from dispenser before re-grasp", + ) + rear_high_xyz = [ + cfg.outlet_xyz_m[0] + self.args.regrasp_rear_x_m, + cfg.outlet_xyz_m[1] + self.args.regrasp_rear_y_m, + cfg.outlet_xyz_m[2] + self.args.regrasp_pre_z_m, + ] + rear_low_xyz = [ + cfg.outlet_xyz_m[0] + self.args.regrasp_rear_x_m, + cfg.outlet_xyz_m[1] + self.args.regrasp_rear_y_m, + cfg.outlet_xyz_m[2], + ] + self.movel( + self.posx(rear_high_xyz, cfg.outlet_zyz_deg), + "re-grasp rear high pre-pose", + self.args.travel_velocity, + self.args.travel_acceleration, + ) + self.movel( + self.posx(rear_low_xyz, cfg.outlet_zyz_deg), + "re-grasp rear lowered pose", + self.args.approach_velocity, + self.args.approach_acceleration, + ) + self.movel(place, "re-grasp forward to cup", self.args.approach_velocity, self.args.approach_acceleration) + self.gripper_command("set_width", self.args.gripper_grasp_width_m, self.args.gripper_grasp_force_n, "soft side-grasp cup") + + pose = self.current_posx() + final_lift = [pose[0], pose[1], pose[2] + self.args.final_lift_m * 1000.0, pose[3], pose[4], pose[5]] + self.movel(final_lift, "lift cup after re-grasp", self.args.travel_velocity, self.args.travel_acceleration) + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Run a minimal dispenser place/press/re-grasp cycle.") + parser.add_argument("--dispenser-ids", default="1x1", help="comma-separated IDs, e.g. 1x1,2x2") + parser.add_argument("--calibration", type=Path, default=DEFAULT_CALIBRATION) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--wait-service-sec", type=float, default=8.0) + parser.add_argument("--motion-timeout-sec", type=float, default=90.0) + parser.add_argument("--x-min", type=float, default=0.10) + parser.add_argument("--x-max", type=float, default=0.90) + 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.85) + parser.add_argument("--joint-min-deg", type=float, default=-360.0) + parser.add_argument("--joint-max-deg", type=float, default=360.0) + parser.add_argument("--j5-min-deg", type=float, default=-135.0) + parser.add_argument("--j5-max-deg", type=float, default=135.0) + parser.add_argument( + "--cup-place-mode", + choices=("joint", "cartesian"), + default="joint", + help="joint uses cup_pre/place_joints_deg; cartesian uses outlet_pose offsets", + ) + parser.add_argument( + "--press-mode", + choices=("joint", "cartesian"), + default="joint", + help="joint uses press_pre/contact_joints_deg; cartesian uses press_pose plus line pump", + ) + parser.add_argument( + "--press-transfer-waypoint", + choices=("none", "color_scan"), + default="color_scan", + help="measured joint waypoint used before/after selected dispenser press moves", + ) + parser.add_argument( + "--press-waypoint-dispenser-ids", + default="3,4", + help="comma-separated dispenser IDs that use the press transfer waypoint", + ) + parser.add_argument("--disable-press-entry-waypoint", action="store_true") + parser.add_argument("--disable-press-exit-waypoint", action="store_true") + parser.add_argument("--disable-joint-branch-unwrap", action="store_true") + parser.add_argument( + "--press-lock-joints", + default="4,6", + help="comma-separated joints copied from PRESS_PRE into PRESS contact command; empty disables", + ) + + parser.add_argument("--cup-pre-x-m", type=float, default=-0.080) + parser.add_argument("--cup-pre-y-m", type=float, default=0.0) + parser.add_argument("--cup-pre-z-m", type=float, default=0.120) + parser.add_argument("--after-release-min-z-m", type=float, default=0.500) + parser.add_argument("--after-release-retreat-x-m", type=float, default=-0.050) + + parser.add_argument("--press-pre-z-m", type=float, default=0.080) + parser.add_argument("--press-depth-m", type=float, default=0.040) + parser.add_argument("--post-press-min-z-m", type=float, default=0.500) + parser.add_argument("--post-press-retreat-x-m", type=float, default=-0.080) + + parser.add_argument("--regrasp-rear-x-m", type=float, default=-0.080) + parser.add_argument("--regrasp-rear-y-m", type=float, default=0.0) + parser.add_argument("--regrasp-pre-z-m", type=float, default=0.120) + parser.add_argument("--final-lift-m", type=float, default=0.100) + + parser.add_argument("--travel-velocity", type=float, default=35.0) + parser.add_argument("--travel-acceleration", type=float, default=50.0) + parser.add_argument("--approach-velocity", type=float, default=20.0) + parser.add_argument("--approach-acceleration", type=float, default=30.0) + parser.add_argument("--press-joint-velocity", type=float, default=20.0) + parser.add_argument("--press-joint-acceleration", type=float, default=30.0) + parser.add_argument("--press-waypoint-velocity", type=float, default=25.0) + parser.add_argument("--press-waypoint-acceleration", type=float, default=35.0) + parser.add_argument("--press-line-velocity", type=float, default=12.0) + parser.add_argument("--press-line-acceleration", type=float, default=18.0) + parser.add_argument("--press-hold-sec", type=float, default=0.15) + + 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("--press-gripper-width-m", type=float, default=0.0) + parser.add_argument("--press-gripper-force-n", type=float, default=30.0) + parser.add_argument("--gripper-grasp-width-m", type=float, default=0.075) + parser.add_argument("--gripper-grasp-force-n", type=float, default=25.0) + parser.add_argument("--gripper-timeout-sec", type=float, default=12.0) + parser.add_argument("--gripper-settle-sec", type=float, default=0.8) + parser.add_argument("--gripper-open-settle-sec", type=float, default=1.2) + + 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() + + +def print_plan(groups: list[tuple[str, int]], args: argparse.Namespace) -> None: + print("[Azas] Minimal dispenser cycle") + print("[Azas] no MoveIt, no /collision_object, no link6/TCP target verification") + print(f"[Azas] dispenser_groups={','.join(f'{dispenser_id}x{count}' for dispenser_id, count in groups)}") + print(f"[Azas] cup_place_mode={args.cup_place_mode}") + print(f"[Azas] press_mode={args.press_mode}") + print( + f"[Azas] press_transfer_waypoint={args.press_transfer_waypoint} " + f"ids={args.press_waypoint_dispenser_ids or '-'} " + f"entry={'off' if args.disable_press_entry_waypoint else 'on'} " + f"exit={'off' if args.disable_press_exit_waypoint else 'on'} " + f"branch_unwrap={'off' if args.disable_joint_branch_unwrap else 'on'} " + f"press_lock_joints={args.press_lock_joints or '-'}" + ) + print( + "[Azas] sequence=cup pre-place -> cup place/open -> cup pre-place/close -> " + "press pre -> press/contact/pre repeat -> press exit -> cup pre-place/open -> cup place/grasp -> lift" + ) + if args.cup_place_mode == "cartesian": + print( + f"[Azas] offsets: cup_pre=({args.cup_pre_x_m:.3f},{args.cup_pre_y_m:.3f},{args.cup_pre_z_m:.3f}) " + f"after_release_retreat_x={args.after_release_retreat_x_m:.3f} " + f"post_press_retreat_x={args.post_press_retreat_x_m:.3f} " + f"regrasp_rear=({args.regrasp_rear_x_m:.3f},{args.regrasp_rear_y_m:.3f},{args.regrasp_pre_z_m:.3f})" + ) + else: + print( + f"[Azas] post_press_escape: min_z={args.post_press_min_z_m:.3f} " + f"retreat_x={args.post_press_retreat_x_m:.3f}; " + f"joint_bounds=[{args.joint_min_deg:.0f},{args.joint_max_deg:.0f}], " + f"j5_bounds=[{args.j5_min_deg:.0f},{args.j5_max_deg:.0f}]" + ) + + +def main() -> int: + args = parse_args() + try: + dispenser_ids = parse_dispenser_ids(args.dispenser_ids) + waypoint_dispenser_ids = parse_dispenser_id_set(args.press_waypoint_dispenser_ids) + press_lock_joint_indexes = parse_joint_index_set(args.press_lock_joints) + except ValueError as exc: + print(f"[FAIL] {exc}", file=sys.stderr) + return 2 + groups = group_consecutive(dispenser_ids) + if args.execute and args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --confirm must be exactly {CONFIRM_PHRASE}", file=sys.stderr) + return 2 + if not args.calibration.is_file(): + print(f"[FAIL] calibration file not found: {args.calibration}", file=sys.stderr) + return 2 + + print_plan(groups, args) + try: + calibrations = [load_dispenser(args.calibration, dispenser_id) for dispenser_id, _ in groups] + press_transfer_waypoint = load_press_transfer_waypoint(args.calibration, args.press_transfer_waypoint) + if args.cup_place_mode == "joint": + for cfg in calibrations: + require_cup_place_joints(cfg) + if args.press_mode == "joint": + for cfg in calibrations: + require_press_joints(cfg) + except ValueError as exc: + print(f"[FAIL] {exc}", file=sys.stderr) + return 2 + for cfg, (_, count) in zip(calibrations, groups): + pre_place = [ + cfg.outlet_xyz_m[0] + args.cup_pre_x_m, + cfg.outlet_xyz_m[1] + args.cup_pre_y_m, + cfg.outlet_xyz_m[2] + args.cup_pre_z_m, + ] + press_pre = [cfg.press_xyz_m[0], cfg.press_xyz_m[1], cfg.press_xyz_m[2] + args.press_pre_z_m] + if args.cup_place_mode == "joint": + cup_pre_place_joints, cup_place_joints = require_cup_place_joints(cfg) + cup_part = ( + f"cup_pre_joints={[round(v, 2) for v in cup_pre_place_joints]} " + f"cup_place_joints={[round(v, 2) for v in cup_place_joints]}" + ) + else: + cup_part = f"cup_place={cfg.outlet_xyz_m} cup_pre={[round(v, 3) for v in pre_place]}" + if args.press_mode == "joint": + press_pre_joints, press_contact_joints = require_press_joints(cfg) + press_locked_joints = lock_joints_to_reference( + press_contact_joints, + press_pre_joints, + press_lock_joint_indexes, + ) + press_command_joints = ( + press_locked_joints + if args.disable_joint_branch_unwrap + else unwrap_joint_target_near(press_pre_joints, press_locked_joints) + ) + waypoint_part = "" + if press_transfer_waypoint is not None and cfg.dispenser_id in waypoint_dispenser_ids: + waypoint_part = f" waypoint={args.press_transfer_waypoint}:{[round(v, 2) for v in press_transfer_waypoint]}" + command_part = "" + if any(abs(raw - command) > 1e-6 for raw, command in zip(press_contact_joints, press_command_joints)): + command_part = f" press_command={[round(v, 2) for v in press_command_joints]}" + press_part = ( + f"press_pre_joints={[round(v, 2) for v in press_pre_joints]} " + f"press_joints={[round(v, 2) for v in press_contact_joints]}" + f"{command_part}" + f"{waypoint_part}" + ) + else: + press_part = ( + f"press_pre={[round(v, 3) for v in press_pre]} " + f"press_joints={[round(v, 2) for v in cfg.press_contact_joints_deg]}" + ) + print( + f"[PLAN] dispenser {cfg.dispenser_id} x{count}: " + f"{cup_part} " + f"{press_part}" + ) + + if not args.execute: + print("[DRY-RUN] --execute not set; no robot or gripper command sent.") + return 0 + + cycle = MinimalCycle(args) + try: + cycle.preflight() + for cfg, (_, count) in zip(calibrations, groups): + cycle.run_group(cfg, count) + except RuntimeError as exc: + print(f"[FAIL] {exc}", file=sys.stderr) + return 1 + finally: + cycle.close() + print("[PASS] minimal dispenser cycle completed") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) 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/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index 56982ed..a045cfc 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -12,6 +12,14 @@ LIVE_GATE_MAX_AGE_SEC="${LIVE_GATE_MAX_AGE_SEC:-600}" REAL_MOTION_CONFIG_CHECK="${REAL_MOTION_CONFIG_CHECK:-${CHECKS_DIR}/check_real_motion_config.sh}" MOTION_HOLD_FILE="${MOTION_HOLD_FILE:-/tmp/azas_motion_hold}" GRASPED_CUP_TEST_MODE="${GRASPED_CUP_TEST_MODE:-false}" +SKIP_CUP_HOLDER_PICK="${SKIP_CUP_HOLDER_PICK:-false}" +CUP_HOLDER_PICK_CONFIG="${CUP_HOLDER_PICK_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/calibration.yaml}" +CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M="${CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M:-0.0}" +# Operational-only offset for the pre-shake re-grasp from cup_holder.side_grip_place. +# 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:-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}" @@ -28,7 +36,7 @@ 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_CYCLES="${SHAKE_CYCLES:-3}" SHAKE_TWIST_RX_DEG="${SHAKE_TWIST_RX_DEG:-6.0}" SHAKE_TWIST_RY_DEG="${SHAKE_TWIST_RY_DEG:-3.0}" SHAKE_TWIST_RZ_DEG="${SHAKE_TWIST_RZ_DEG:-22.0}" @@ -40,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}" @@ -50,9 +60,9 @@ JOINT_SHAKE_BASE_J4_DEG="${JOINT_SHAKE_BASE_J4_DEG:-0.0}" JOINT_SHAKE_BASE_J5_DEG="${JOINT_SHAKE_BASE_J5_DEG:-70.0}" JOINT_SHAKE_BASE_J6_DEG="${JOINT_SHAKE_BASE_J6_DEG:-0.0}" JOINT_SHAKE_J3_AMPLITUDE_DEG="${JOINT_SHAKE_J3_AMPLITUDE_DEG:-0.0}" -JOINT_SHAKE_J4_AMPLITUDE_DEG="${JOINT_SHAKE_J4_AMPLITUDE_DEG:-25.0}" -JOINT_SHAKE_J5_AMPLITUDE_DEG="${JOINT_SHAKE_J5_AMPLITUDE_DEG:-30.0}" -JOINT_SHAKE_J6_AMPLITUDE_DEG="${JOINT_SHAKE_J6_AMPLITUDE_DEG:-37.0}" +JOINT_SHAKE_J4_AMPLITUDE_DEG="${JOINT_SHAKE_J4_AMPLITUDE_DEG:-18.0}" +JOINT_SHAKE_J5_AMPLITUDE_DEG="${JOINT_SHAKE_J5_AMPLITUDE_DEG:-20.0}" +JOINT_SHAKE_J6_AMPLITUDE_DEG="${JOINT_SHAKE_J6_AMPLITUDE_DEG:-24.0}" JOINT_SHAKE_J1_MIN_DEG="${JOINT_SHAKE_J1_MIN_DEG:--20.0}" JOINT_SHAKE_J1_MAX_DEG="${JOINT_SHAKE_J1_MAX_DEG:-5.0}" JOINT_SHAKE_J2_MIN_DEG="${JOINT_SHAKE_J2_MIN_DEG:--80.0}" @@ -63,10 +73,10 @@ JOINT_SHAKE_MAX_SINGLE_DELTA_DEG="${JOINT_SHAKE_MAX_SINGLE_DELTA_DEG:-75.0}" APPROACH_JOINT_VELOCITY="${APPROACH_JOINT_VELOCITY:-18.0}" APPROACH_JOINT_ACCELERATION="${APPROACH_JOINT_ACCELERATION:-22.0}" APPROACH_JOINT_TIME="${APPROACH_JOINT_TIME:-2.6}" -SHAKE_JOINT_VELOCITY="${SHAKE_JOINT_VELOCITY:-180.0}" -SHAKE_JOINT_ACCELERATION="${SHAKE_JOINT_ACCELERATION:-260.0}" +SHAKE_JOINT_VELOCITY="${SHAKE_JOINT_VELOCITY:-90.0}" +SHAKE_JOINT_ACCELERATION="${SHAKE_JOINT_ACCELERATION:-120.0}" SHAKE_JOINT_TIME="${SHAKE_JOINT_TIME:-0.0}" -JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S="${JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S:-225.0}" +JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S="${JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S:-130.0}" VERIFY_JOINT_TARGETS="${VERIFY_JOINT_TARGETS:-true}" JOINT_TARGET_TOLERANCE_DEG="${JOINT_TARGET_TOLERANCE_DEG:-8.0}" JOINT_TARGET_WAIT_EXTRA_SEC="${JOINT_TARGET_WAIT_EXTRA_SEC:-3.0}" @@ -74,10 +84,11 @@ 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 완료 확인 -> RG2가 컵 몸통/홀더 쪽을 안정적으로 잡은 상태 확인 -> 들어 올림/관절 쉐이킹 실행." -echo "[Azas] 주의: 이 스크립트는 컵 좌표를 새로 만들지 않으며, GRASPED_CUP_TEST_MODE=true에서는 컵이 이미 잡힌 상태를 전제로 합니다." +echo "[Azas] 순서: 컵홀더 place 완료 확인 -> 컵홀더 측정 pose로 RG2 side-grip 픽업 -> 들어 올림 -> 관절 쉐이킹 실행." +echo "[Azas] 주의: 이 스크립트는 컵 좌표를 새로 만들지 않으며, calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다." if [[ -f "${MOTION_HOLD_FILE}" ]]; then echo "[Azas] Refusing real robot shake: motion hold is active." @@ -149,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 @@ -240,7 +264,7 @@ else echo "[Azas] Strict live gate stamp: ${LIVE_GATE_STAMP} age=${age_sec}s" fi echo "[Azas] Continue only if ALL are true:" -echo " - cup is already grasped securely" +echo " - cup-holder pick completed and cup is grasped securely" echo " - e-stop is reachable" echo " - no person is inside the robot workspace" echo " - dispenser, tumbler, cable, table, and camera mount collision risks were checked" @@ -251,13 +275,58 @@ 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 fi -exec ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ +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 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 \ + --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 + +# 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 \ @@ -322,4 +391,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/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 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..b14d619 --- /dev/null +++ b/tools/run/run_somyeong_cup_uprighting_direct.sh @@ -0,0 +1,67 @@ +#!/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}" +EXIT_AFTER_PICK="${EXIT_AFTER_PICK:-false}" +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:-1}" + +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} exit_after_pick=${EXIT_AFTER_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 + 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}" \ + exit_after_pick:="${EXIT_AFTER_PICK}" \ + skip_initial_home_move:="${SKIP_INITIAL_HOME_MOVE}" \ + publish_hand_eye_tf:="${PUBLISH_HAND_EYE_TF}" \ + moveit_controller_name:="${MOVEIT_CONTROLLER_NAME}" \ + controller_action_name:="${CONTROLLER_ACTION_NAME}" 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..f316cc0 --- /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:-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}" + +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 diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh new file mode 100755 index 0000000..e190233 --- /dev/null +++ b/tools/run/run_tmux_logic_sequence.sh @@ -0,0 +1,257 @@ +#!/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}" +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}" + +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 ROS_LOCALHOST_ONLY + +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:=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 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_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 \ + 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.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 \ + 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 + ) +} + +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:="${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:=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 \ + 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/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh new file mode 100755 index 0000000..61783f4 --- /dev/null +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -0,0 +1,75 @@ +#!/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}" +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}" +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 + 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} 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 \ + 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 \ + 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}" \ + 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/run_voice_dispenser_sim_m0609.sh b/tools/run/run_voice_dispenser_sim_m0609.sh new file mode 100755 index 0000000..b165943 --- /dev/null +++ b/tools/run/run_voice_dispenser_sim_m0609.sh @@ -0,0 +1,143 @@ +#!/usr/bin/env bash +set -euo pipefail + +# One-command virtual sim for the voice -> dispenser chain on the Doosan M0609. +# +# Brings up, in one terminal: +# 1) virtual Doosan M0609 (MoveIt + RViz) -> provides /motion/* services +# 2) Azas collision scene (safety zone + dispenser box) +# 3) azas_voice stack with the dispenser executor (hardware execution enabled) +# incl. the voice screen web UI on VOICE_PORT (default 8090) +# 4) the kiosk web UI on KIOSK_PORT (default 8080) unless WITH_KIOSK=false +# +# Order from http://localhost:8080 (kiosk: pick menu -> 시작) or +# http://localhost:8090 (voice: say an order -> "응"). +# +# Then publish a confirmed recipe decision to drive the arm to the dispensers, e.g.: +# ros2 topic pub --once /azas/voice/confirmed_recipe_decision std_msgs/msg/String \ +# '{data: "{\"intent\":\"make_cocktail\",\"confirmed\":true,\"recipe_id\":\"sim\",\"dispenser_ids\":[\"red\",\"blue\"],\"dispenser_amounts\":{\"red\":2,\"blue\":1}}"}' +# +# Override behaviour with env vars (see defaults below). + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" + +SERVICE_PREFIX="${SERVICE_PREFIX:-/}" # virtual stack exposes /motion/* with no namespace +REQUIRE_TCP="${REQUIRE_TCP:-false}" # sim has no named TCP -> keep false +USE_TTS="${USE_TTS:-false}" # silence audio for a quiet sim by default +SERVICE_WAIT_SEC="${SERVICE_WAIT_SEC:-60}" # how long to wait for the virtual robot +AUTO_ORDER="${AUTO_ORDER:-false}" # set to true to auto-fire a red/blue test order +WITH_KIOSK="${WITH_KIOSK:-true}" # also launch the kiosk web UI on KIOSK_PORT +KIOSK_PORT="${KIOSK_PORT:-8080}" +VOICE_PORT="${VOICE_PORT:-8090}" # voice screen web UI port +# Point the kiosk "제어 상태" display at the dispenser executor's status so it +# reflects the real run (queued/starting/completed) instead of staying "대기". +KIOSK_STATUS_TOPIC="${KIOSK_STATUS_TOPIC:-/azas/voice/dispenser_execution_status}" + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +source "${ROOT_DIR}/install/setup.bash" +set -u + +robot_pid="" +scene_pid="" +voice_pid="" +kiosk_pid="" + +terminate_tree() { + local pid="$1" + [[ -z "${pid}" ]] && return + pkill -TERM -P "${pid}" 2>/dev/null || true + kill "${pid}" 2>/dev/null || true + sleep 1 + pkill -KILL -P "${pid}" 2>/dev/null || true + kill -KILL "${pid}" 2>/dev/null || true +} + +cleanup() { + echo "[Azas] Shutting down voice-dispenser sim..." + terminate_tree "${kiosk_pid}" + terminate_tree "${voice_pid}" + terminate_tree "${scene_pid}" + terminate_tree "${robot_pid}" + wait "${kiosk_pid}" "${voice_pid}" "${scene_pid}" "${robot_pid}" 2>/dev/null || true +} +trap cleanup EXIT INT TERM + +echo "[Azas] (1/3) Starting virtual Doosan M0609 (MoveIt + RViz)..." +bash "${ROOT_DIR}/tools/run/run_doosan_virtual_m0609.sh" & +robot_pid=$! + +echo "[Azas] Waiting for the virtual robot motion services (up to ${SERVICE_WAIT_SEC}s)..." +deadline=$((SECONDS + SERVICE_WAIT_SEC)) +until ros2 service list 2>/dev/null | grep -q "/motion/move_joint"; do + if (( SECONDS >= deadline )); then + echo "[Azas] ERROR: /motion/move_joint never appeared. Aborting." >&2 + exit 1 + fi + if ! kill -0 "${robot_pid}" 2>/dev/null; then + echo "[Azas] ERROR: virtual robot process exited early. Aborting." >&2 + exit 1 + fi + sleep 1 +done +echo "[Azas] Virtual robot is up (/motion/move_joint found)." + +echo "[Azas] (2/3) Starting Azas collision scene (safety zone + dispenser box)..." +ros2 launch azas_bringup workspace_collision_scene.launch.py & +scene_pid=$! +sleep 2 + +echo "[Azas] (3/3) Starting azas_voice stack with dispenser executor..." +ros2 launch azas_voice azas_voice.launch.py \ + use_dispenser_executor:=true \ + enable_dispenser_hardware_execution:=true \ + dispenser_service_prefix:="${SERVICE_PREFIX}" \ + dispenser_require_tcp_for_taught_posx:="${REQUIRE_TCP}" \ + run_voice_screen:=true \ + voice_screen_port:="${VOICE_PORT}" \ + use_tts:="${USE_TTS}" & +voice_pid=$! +sleep 3 + +if [[ "${WITH_KIOSK}" == "true" ]]; then + echo "[Azas] (+) Starting kiosk web UI on port ${KIOSK_PORT}..." + ros2 launch azas_kiosk azas_kiosk.launch.py \ + host:=0.0.0.0 port:="${KIOSK_PORT}" \ + cocktail_status_topic:="${KIOSK_STATUS_TOPIC}" & + kiosk_pid=$! + sleep 2 +fi + +echo "" +echo "[Azas] ============================================================" +echo "[Azas] Voice-dispenser sim is up. Order through the web UIs:" +echo "[Azas]" +if [[ "${WITH_KIOSK}" == "true" ]]; then + echo "[Azas] Kiosk : http://localhost:${KIOSK_PORT} (click a menu, then click 시작/Start)" +fi +echo "[Azas] Voice : http://localhost:${VOICE_PORT} (say/type an order, then \"응\")" +echo "[Azas]" +echo "[Azas] A click/utterance alone only stages the order; the CONFIRM step" +echo "[Azas] (시작 button / \"응\") is what triggers the robot." +echo "[Azas]" +echo "[Azas] Or fire a confirmed order directly:" +echo "[Azas] ros2 topic pub --once /azas/voice/confirmed_recipe_decision std_msgs/msg/String \\" +echo "[Azas] '{data: \"{\\\"intent\\\":\\\"make_cocktail\\\",\\\"confirmed\\\":true,\\\"recipe_id\\\":\\\"sim\\\",\\\"dispenser_ids\\\":[\\\"red\\\",\\\"blue\\\"],\\\"dispenser_amounts\\\":{\\\"red\\\":2,\\\"blue\\\":1}}\"}'" +echo "[Azas]" +echo "[Azas] Watch status: ros2 topic echo /azas/voice/dispenser_execution_status" +echo "[Azas] Ctrl+C here stops the whole sim." +echo "[Azas] ============================================================" + +if [[ "${AUTO_ORDER}" == "true" ]]; then + echo "[Azas] AUTO_ORDER=true -> firing a red(x2)+blue(x1) test order in 3s..." + sleep 3 + ros2 topic pub --once /azas/voice/confirmed_recipe_decision std_msgs/msg/String \ + '{data: "{\"intent\":\"make_cocktail\",\"confirmed\":true,\"recipe_id\":\"sim\",\"dispenser_ids\":[\"red\",\"blue\"],\"dispenser_amounts\":{\"red\":2,\"blue\":1}}"}' || true +fi + +# Keep the sim alive until any component exits or the user hits Ctrl+C. +wait -n "${robot_pid}" "${scene_pid}" "${voice_pid}" ${kiosk_pid:+"${kiosk_pid}"} 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..7e9048d --- /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:-1}" + +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/show_measured_recipe_joint_preview_rviz.sh b/tools/run/show_measured_recipe_joint_preview_rviz.sh new file mode 100755 index 0000000..af622b1 --- /dev/null +++ b/tools/run/show_measured_recipe_joint_preview_rviz.sh @@ -0,0 +1,127 @@ +#!/usr/bin/env bash +set -euo pipefail + +# RViz-only measured joint preview. This starts robot_state_publisher + RViz and +# publishes /joint_states from calibration.yaml. It does not call Doosan motion, +# MoveJoint, MoveLine, or gripper services. + +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}" +SAFETY_CONFIG="${SAFETY_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/safety.yaml}" +DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml}" +CALIBRATION_CONFIG="${CALIBRATION_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/calibration.yaml}" +ROBOT_COLOR="${ROBOT_COLOR:-white}" +PUBLISH_RATE="${PUBLISH_RATE:-60.0}" +SEGMENT_SECONDS="${SEGMENT_SECONDS:-4.0}" +JOINT_VELOCITY_DEG_S="${JOINT_VELOCITY_DEG_S:-40.0}" +HOLD_SECONDS="${HOLD_SECONDS:-1.0}" +SHOW_WORKSPACE_SAFETY="${SHOW_WORKSPACE_SAFETY:-true}" +SHOW_FULL_COLLISION_SCENE="${SHOW_FULL_COLLISION_SCENE:-true}" +SHOW_MEASURED_DISPENSER_COLLISION="${SHOW_MEASURED_DISPENSER_COLLISION:-true}" +SHOW_LINK6_GRIPPER="${SHOW_LINK6_GRIPPER:-true}" +PUBLISH_WORKSPACE_COLLISION_OBJECTS="${PUBLISH_WORKSPACE_COLLISION_OBJECTS:-true}" +PUBLISH_DISPENSER_COLLISION_OBJECTS="${PUBLISH_DISPENSER_COLLISION_OBJECTS:-true}" +RESET_STALE_PREVIEW_NODES="${RESET_STALE_PREVIEW_NODES:-true}" +START_FULL_COLLISION_MARKERS="${START_FULL_COLLISION_MARKERS:-false}" +PREVIEW_ARGS=("$@") +if [[ ${#PREVIEW_ARGS[@]} -gt 0 && "${PREVIEW_ARGS[0]}" != --* ]]; then + PREVIEW_ARGS=("--dispenser-ids" "${PREVIEW_ARGS[@]}") +fi +SESSION_NAME="${SESSION_NAME:-azas-measured-joint-preview}" + +mkdir -p "${LOG_DIR}" + +DESC_PID="" +COLLISION_PID="" +JOINT_PID="" +RVIZ_PID="" + +cleanup() { + for pid in "${JOINT_PID}" "${COLLISION_PID}" "${DESC_PID}" "${RVIZ_PID}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + fi + done +} +trap cleanup EXIT INT TERM + +cd "${ROOT_DIR}" +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" +else + source "${ROOT_DIR}/install/local_setup.bash" +fi +set -u + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +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 +pkill -f 'preview_measured_dispenser_recipe_rviz.py' 2>/dev/null || true +pkill -f 'rule_motion_joint_preview_node' 2>/dev/null || true +pkill -f 'm0609_shake_joint_state_node' 2>/dev/null || true +if [[ "${RESET_STALE_PREVIEW_NODES}" == "true" || "${RESET_STALE_PREVIEW_NODES}" == "1" ]]; then + pkill -f 'dispenser_sequence_preview_node' 2>/dev/null || true + pkill -f 'workspace_collision_scene_node' 2>/dev/null || true + pkill -f 'measured_dispenser_collision_scene_node' 2>/dev/null || true + pkill -f 'collision_scene_rviz_publisher' 2>/dev/null || true + pkill -f 'link6_gripper_collision_node' 2>/dev/null || true + pkill -f '__node:=m0609_robot_state_publisher' 2>/dev/null || true +fi + +ros2 launch "${ROOT_DIR}/src/azas_bringup/launch/measured_joint_preview_display.launch.py" \ + use_rviz:=false \ + robot_color:="${ROBOT_COLOR}" \ + show_workspace_safety:="${SHOW_WORKSPACE_SAFETY}" \ + show_measured_dispenser_collision:="${SHOW_MEASURED_DISPENSER_COLLISION}" \ + show_full_collision_scene:="${SHOW_FULL_COLLISION_SCENE}" \ + show_link6_gripper:="${SHOW_LINK6_GRIPPER}" \ + publish_workspace_collision_objects:="${PUBLISH_WORKSPACE_COLLISION_OBJECTS}" \ + publish_dispenser_collision_objects:="${PUBLISH_DISPENSER_COLLISION_OBJECTS}" \ + safety_config_path:="${SAFETY_CONFIG}" \ + dispenser_collision_config_path:="${DISPENSER_COLLISION_CONFIG}" \ + calibration_path:="${CALIBRATION_CONFIG}" \ + >"${LOG_DIR}/measured_joint_preview_description.log" 2>&1 & +DESC_PID=$! + +COLLISION_PID="" +if [[ "${START_FULL_COLLISION_MARKERS}" == "true" ]]; then + python3 tools/run/publish_collision_scene_rviz.py \ + >"${LOG_DIR}/measured_joint_preview_collision.log" 2>&1 & + COLLISION_PID=$! +fi + +python3 tools/run/preview_measured_dispenser_recipe_rviz.py \ + --rate-hz "${PUBLISH_RATE}" \ + --segment-seconds "${SEGMENT_SECONDS}" \ + --joint-velocity-deg-s "${JOINT_VELOCITY_DEG_S}" \ + --hold-seconds "${HOLD_SECONDS}" \ + "${PREVIEW_ARGS[@]}" \ + >"${LOG_DIR}/measured_joint_preview_joints.log" 2>&1 & +JOINT_PID=$! + +rviz2 -d "${RVIZ_CONFIG}" \ + >"${LOG_DIR}/measured_joint_preview_rviz.log" 2>&1 & +RVIZ_PID=$! + +echo "[Azas] RViz measured joint preview started." +echo "[Azas] Session marker: ${SESSION_NAME}" +echo "[Azas] Args: ${PREVIEW_ARGS[*]:-(default recipe/color map)}" +echo "[Azas] safety_config=${SAFETY_CONFIG}" +echo "[Azas] dispenser_collision_config=${DISPENSER_COLLISION_CONFIG}" +echo "[Azas] calibration_config=${CALIBRATION_CONFIG}" +echo "[Azas] preview_joint_velocity_deg_s=${JOINT_VELOCITY_DEG_S}" +echo "[Azas] PIDs: description=${DESC_PID} collision=${COLLISION_PID} joints=${JOINT_PID} rviz=${RVIZ_PID}" +echo "[Azas] Logs: ${LOG_DIR}/measured_joint_preview_*.log" + +wait "${RVIZ_PID}" diff --git a/tools/run/start_azas_tmux_stack.sh b/tools/run/start_azas_tmux_stack.sh new file mode 100755 index 0000000..f0b7ff4 --- /dev/null +++ b/tools/run/start_azas_tmux_stack.sh @@ -0,0 +1,77 @@ +#!/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:-1}" +FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +FORCE_RESTART="${FORCE_RESTART:-false}" +CLEAN_FASTDDS_SHM="${CLEAN_FASTDDS_SHM:-false}" + +cd "${ROOT}" +mkdir -p "${ROOT}/log/tmux_logic" /tmp/azas_ros_logs + +if tmux has-session -t "${SESSION}" >/dev/null 2>&1; then + if tmux list-windows -t "${SESSION}" -F '#{window_name}' | grep -qx 'side_grip' \ + && pgrep -f '[y]olo_cup_pick_node' >/dev/null 2>&1 \ + && [[ "${FORCE_RESTART}" != "true" ]]; then + echo "[Azas] BLOCKED: ${SESSION}:side_grip is running. Stop side_grip first or set FORCE_RESTART=true." >&2 + exit 3 + fi + + tmux list-panes -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..20}; do + if ! tmux has-session -t "${SESSION}" >/dev/null 2>&1; then + break + fi + if ! pgrep -f "tmux.*${SESSION}|run_doosan_real_m0609|rg2_trigger.launch.py|rs_launch.py camera_name:=camera|joint_state_relay.py" >/dev/null 2>&1; then + break + fi + sleep 0.2 + done + tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true +fi + +# 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=) + +if [[ "${CLEAN_FASTDDS_SHM}" == "true" ]]; then + if pgrep -f 'ros2|realsense2_camera_node|dsr_controller2|move_group|rviz2|rg2_gripper_node|yolo_cup_pick_node' >/dev/null 2>&1; then + echo "[Azas] SKIP FastDDS SHM cleanup: ROS processes are still running." >&2 + else + rm -f /dev/shm/fastrtps_* >/dev/null 2>&1 || true + echo "[Azas] cleaned /dev/shm/fastrtps_*" + fi +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" +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}" diff --git a/tools/run/start_azas_voice_stack.sh b/tools/run/start_azas_voice_stack.sh new file mode 100755 index 0000000..fc8d6d0 --- /dev/null +++ b/tools/run/start_azas_voice_stack.sh @@ -0,0 +1,75 @@ +#!/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_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}" + +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_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}" +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] 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}" + +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_all.sh b/tools/run/stop_azas_all.sh new file mode 100755 index 0000000..990c0f8 --- /dev/null +++ b/tools/run/stop_azas_all.sh @@ -0,0 +1,118 @@ +#!/usr/bin/env bash +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/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 +# own ancestry. +# +# Usage: +# bash tools/run/stop_azas_all.sh # stop everything + shm clean +# DRY_RUN=1 bash tools/run/stop_azas_all.sh # only print what would die +# KILL_PANEL=1 bash tools/run/stop_azas_all.sh # also stop the control panel +# CLEAN_FASTDDS_SHM=0 ... # skip /dev/shm cleanup + +DRY_RUN="${DRY_RUN:-0}" +KILL_PANEL="${KILL_PANEL:-0}" +CLEAN_FASTDDS_SHM="${CLEAN_FASTDDS_SHM:-1}" +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|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 + PROTECT_PATTERN="${PROTECT_PATTERN}|robot_pipeline_control_server\.py|run_robot_pipeline_control_panel\.sh" +fi + +self_and_ancestors() { + local pid=$$ + while [[ -n "${pid}" && "${pid}" -gt 1 ]]; do + echo "${pid}" + pid="$(ps -o ppid= -p "${pid}" 2>/dev/null | tr -d ' ')" || break + done +} +PROTECTED_PIDS=" $(self_and_ancestors | tr '\n' ' ') " + +collect_pids() { + 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 +} + +signal_pids() { + local sig="$1" + shift + local pid + for pid in "$@"; do + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] kill -${sig} ${pid} :: $(ps -p "${pid}" -o args= 2>/dev/null | cut -c1-140)" + else + kill "-${sig}" "${pid}" 2>/dev/null || true + fi + done +} + +# 1) Gracefully stop the azas tmux logic sessions (C-c, brief wait, kill). +for session in ${SESSIONS}; do + if tmux has-session -t "${session}" >/dev/null 2>&1; then + echo "[Azas] stopping tmux session: ${session}" + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + tmux list-windows -t "${session}" -F "[DRY_RUN] would kill window ${session}:#{window_name}" + continue + 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 + sleep 2 + tmux kill-session -t "${session}" >/dev/null 2>&1 || true + fi +done + +# 2) TERM every matched ROS process, wait, then KILL survivors. +mapfile -t targets < <(collect_pids) +if [[ "${#targets[@]}" -gt 0 ]]; then + echo "[Azas] stopping ${#targets[@]} ROS-related processes" + signal_pids TERM "${targets[@]}" + if [[ "${DRY_RUN}" != "1" && "${DRY_RUN}" != "true" ]]; then + deadline=$((SECONDS + GRACE_SEC)) + while [[ ${SECONDS} -lt ${deadline} ]]; do + mapfile -t remaining < <(collect_pids) + [[ "${#remaining[@]}" -eq 0 ]] && break + sleep 0.5 + done + mapfile -t remaining < <(collect_pids) + if [[ "${#remaining[@]}" -gt 0 ]]; then + echo "[Azas] force-killing ${#remaining[@]} survivors" + signal_pids KILL "${remaining[@]}" + sleep 0.5 + fi + fi +else + echo "[Azas] no ROS-related processes found" +fi + +# 3) Clean stale FastDDS shared memory once nothing ROS-related is left. +if [[ "${CLEAN_FASTDDS_SHM}" == "1" || "${CLEAN_FASTDDS_SHM}" == "true" ]]; then + mapfile -t remaining < <(collect_pids) + if [[ "${#remaining[@]}" -gt 0 ]]; then + echo "[Azas] SKIP FastDDS SHM cleanup: ${#remaining[@]} ROS processes still alive" >&2 + elif [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] would remove $(ls /dev/shm 2>/dev/null | grep -c '^fastrtps_' || true) /dev/shm/fastrtps_* segments" + else + count="$(ls /dev/shm 2>/dev/null | grep -c '^fastrtps_' || true)" + rm -f /dev/shm/fastrtps_* >/dev/null 2>&1 || true + echo "[Azas] removed ${count} stale /dev/shm/fastrtps_* segments" + fi +fi + +echo "[Azas] stop_azas_all done. Restart with: bash tools/run/start_azas_tmux_stack.sh" diff --git a/tools/run/stop_azas_voice_stack.sh b/tools/run/stop_azas_voice_stack.sh new file mode 100755 index 0000000..3f530be --- /dev/null +++ b/tools/run/stop_azas_voice_stack.sh @@ -0,0 +1,53 @@ +#!/usr/bin/env bash +# azas-voice tmux 세션(로봇/그리퍼/카메라/음성스택)과 수동으로 띄운 +# azas_voice 노드를 정리한다. +set -euo pipefail + +SESSION="${SESSION:-azas-voice}" +GRACE_SEC="${GRACE_SEC:-3}" + +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' + +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 +} + +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 + + 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/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/run/verify_voice_recommend_confirm.py b/tools/run/verify_voice_recommend_confirm.py new file mode 100644 index 0000000..05696fa --- /dev/null +++ b/tools/run/verify_voice_recommend_confirm.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 +"""Verify the recommend -> "응" -> execute chain makes EXACTLY the recommended recipe. + +Flow checked: + 1. publish a recommendation utterance ("추천해줘") to the STT topic + 2. capture what the mapper recommended on /azas/voice/recipe_decision + 3. publish a confirm utterance ("응") to the STT topic + 4. capture /azas/voice/confirmed_recipe_decision + 5. assert recipe_id / dispenser_ids / dispenser_amounts are carried through UNCHANGED + 6. (if the executor is running) check the queued targets match the recommendation + +Prereq: the voice stack must be running (recipe_mapper + conversation_manager), +e.g. via tools/run/run_voice_dispenser_sim_m0609.sh. + +Usage: + source /opt/ros/humble/setup.bash && source install/setup.bash + python3 tools/run/verify_voice_recommend_confirm.py + +Env overrides: + STT_TOPIC (default /stt_result) + RECOMMEND_UTTERANCE (default "추천해줘") + CONFIRM_UTTERANCE (default "응") +""" +import json +import os +import sys +import time + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +STT_TOPIC = os.environ.get("STT_TOPIC", "/stt_result") +RECOMMEND = os.environ.get("RECOMMEND_UTTERANCE", "추천해줘") +CONFIRM = os.environ.get("CONFIRM_UTTERANCE", "응") +MAX_REPEATS = 3 + + +def dispenser_ids(decision): + return [str(x).strip() for x in (decision.get("dispenser_ids") or [])] + + +def expected_targets(decision): + """Mirror requests_from_decision: expand dispenser_ids by clamped amounts.""" + amounts = decision.get("dispenser_amounts") or {} + out = [] + for did in dispenser_ids(decision): + try: + amount = int(amounts.get(did, 1)) + except (TypeError, ValueError): + amount = 1 + amount = max(0, min(amount, MAX_REPEATS)) + out += [did] * amount + return out + + +class Verifier(Node): + def __init__(self): + super().__init__("verify_voice_recommend_confirm") + self.recommended = None + self.confirmed = None + self.status_msgs = [] + self.create_subscription(String, "/azas/voice/recipe_decision", self._on_recipe, 10) + self.create_subscription( + String, "/azas/voice/confirmed_recipe_decision", self._on_confirmed, 10 + ) + self.create_subscription( + String, "/azas/voice/dispenser_execution_status", self._on_status, 10 + ) + self.pub = self.create_publisher(String, STT_TOPIC, 10) + + def _on_recipe(self, msg): + try: + decision = json.loads(msg.data) + except json.JSONDecodeError: + return + # Only the recommendation (make_cocktail); ignore the "confirm" echo. + if decision.get("intent") == "make_cocktail": + self.recommended = decision + + def _on_confirmed(self, msg): + try: + self.confirmed = json.loads(msg.data) + except json.JSONDecodeError: + pass + + def _on_status(self, msg): + try: + self.status_msgs.append(json.loads(msg.data)) + except json.JSONDecodeError: + pass + + +def spin_until(node, predicate, timeout_sec): + deadline = time.monotonic() + timeout_sec + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if predicate(): + return True + return False + + +def main(): + rclpy.init() + node = Verifier() + failures = [] + + print(f"[verify] waiting for a subscriber on {STT_TOPIC} (recipe mapper)...") + if not spin_until(node, lambda: node.pub.get_subscription_count() > 0, 15.0): + print(f"[verify] FAIL: nothing subscribed to {STT_TOPIC}. Is the voice stack running?") + node.destroy_node() + rclpy.shutdown() + return 1 + + print(f"[verify] (1) recommend -> {RECOMMEND!r}") + node.pub.publish(String(data=RECOMMEND)) + if not spin_until(node, lambda: node.recommended is not None, 10.0): + print("[verify] FAIL: no recommendation on /azas/voice/recipe_decision.") + node.destroy_node() + rclpy.shutdown() + return 1 + rec = node.recommended + print( + f"[verify] recommended: recipe_id={rec.get('recipe_id')} " + f"dispenser_ids={dispenser_ids(rec)} amounts={rec.get('dispenser_amounts')}" + ) + + time.sleep(1.0) + print(f"[verify] (2) confirm -> {CONFIRM!r}") + node.pub.publish(String(data=CONFIRM)) + if not spin_until(node, lambda: node.confirmed is not None, 10.0): + print("[verify] FAIL: no /azas/voice/confirmed_recipe_decision (confirm unrecognized or no pending).") + node.destroy_node() + rclpy.shutdown() + return 1 + conf = node.confirmed + print( + f"[verify] confirmed: recipe_id={conf.get('recipe_id')} " + f"dispenser_ids={dispenser_ids(conf)} amounts={conf.get('dispenser_amounts')} " + f"confirmed={conf.get('confirmed')}" + ) + + print("[verify] (3) compare recommended vs confirmed (must be identical):") + if conf.get("recipe_id") != rec.get("recipe_id"): + failures.append("recipe_id changed") + if dispenser_ids(conf) != dispenser_ids(rec): + failures.append("dispenser_ids changed") + if (conf.get("dispenser_amounts") or None) != (rec.get("dispenser_amounts") or None): + failures.append("dispenser_amounts changed") + if conf.get("confirmed") is not True: + failures.append("confirmed flag not true") + for f in failures: + print(f"[verify] ✗ {f}") + if not failures: + print("[verify] ✓ recipe_id, dispenser_ids, dispenser_amounts all preserved") + + print("[verify] (4) executor queued targets vs recommendation:") + spin_until(node, lambda: any(s.get("status") == "queued" for s in node.status_msgs), 8.0) + queued = next((s for s in node.status_msgs if s.get("status") == "queued"), None) + if queued is None: + print("[verify] (no 'queued' status — executor disabled, or all amounts 0)") + else: + want = expected_targets(conf) + got = queued.get("targets") + print(f"[verify] queued targets={got} expected={want}") + if got != want: + failures.append("executor targets != recommended expansion") + print("[verify] ✗ executor targets differ from recommendation") + else: + print("[verify] ✓ executor will press exactly the recommended dispensers") + + print() + if failures: + print("[verify] RESULT: FAIL ✗ -", "; ".join(failures)) + else: + print("[verify] RESULT: PASS ✅ — '응' makes exactly what was recommended") + + node.destroy_node() + rclpy.shutdown() + return 1 if failures else 0 + + +if __name__ == "__main__": + sys.exit(main()) 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..08cadb9 --- /dev/null +++ b/tools/run/wait_for_lid_grip_status.py @@ -0,0 +1,156 @@ +#!/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", + ) + 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], + *, + 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) + 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 + 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 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() + 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, + 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: + 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()) 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/setup/bootstrap_team_pc.sh b/tools/setup/bootstrap_team_pc.sh new file mode 100755 index 0000000..147ac82 --- /dev/null +++ b/tools/setup/bootstrap_team_pc.sh @@ -0,0 +1,157 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_PATH="$(readlink -f "${BASH_SOURCE[0]}")" +SCRIPT_DIR="$(cd "$(dirname "$SCRIPT_PATH")" && pwd)" +ROOT="$(cd "$SCRIPT_DIR/../.." && pwd)" +REPORT="${REPORT:-/tmp/azas_team_pc_bootstrap_report.txt}" +ROS_SETUP="/opt/ros/humble/setup.bash" +INSTALL_SETUP="$ROOT/install/local_setup.bash" +REQUIRED_BRANCH="${REQUIRED_BRANCH:-develop}" + +cd "$ROOT" +: >"$REPORT" + +log() { + echo "$*" | tee -a "$REPORT" +} + +run() { + log "" + log "[RUN] $*" + "$@" 2>&1 | tee -a "$REPORT" +} + +check_file() { + local path="$1" + if [[ -e "$path" ]]; then + log "[OK] $path" + else + log "[FAIL] missing: $path" + return 1 + fi +} + +check_cmd() { + local cmd="$1" + if command -v "$cmd" >/dev/null 2>&1; then + log "[OK] command: $cmd ($(command -v "$cmd"))" + else + log "[FAIL] missing command: $cmd" + return 1 + fi +} + +check_ros_pkg() { + local pkg="$1" + if ros2 pkg prefix "$pkg" >/dev/null 2>&1; then + log "[OK] ROS package: $pkg -> $(ros2 pkg prefix "$pkg")" + else + log "[FAIL] missing ROS package: $pkg" + return 1 + fi +} + +log "[Azas team bootstrap] workspace=$ROOT" +log "[Azas team bootstrap] report=$REPORT" + +branch="$(git branch --show-current 2>/dev/null || true)" +log "[INFO] git branch=${branch:-}" +if [[ -n "$REQUIRED_BRANCH" && "$branch" != "$REQUIRED_BRANCH" ]]; then + log "[FAIL] expected branch '$REQUIRED_BRANCH' but current branch is '${branch:-}'" + log " Fix with: git switch $REQUIRED_BRANCH && git pull --ff-only origin $REQUIRED_BRANCH" + exit 1 +fi + +run git fetch origin --prune +run git status --short --branch + +if [[ -n "$(git status --porcelain)" ]]; then + log "[FAIL] worktree has local changes. Commit/stash them before bootstrapping another PC." + exit 1 +fi + +run git pull --ff-only origin "$REQUIRED_BRANCH" + +check_file "$ROS_SETUP" +check_cmd colcon + +if ! command -v rosdep >/dev/null 2>&1; then + log "[FAIL] missing command: rosdep" + log " Install example: sudo apt install -y python3-rosdep && sudo rosdep init || true && rosdep update" + exit 1 +fi +log "[OK] command: rosdep ($(command -v rosdep))" + +# shellcheck source=/opt/ros/humble/setup.bash +source "$ROS_SETUP" + +log "" +log "[Azas team bootstrap] installing rosdep dependencies" +rosdep install --from-paths src --ignore-src -r -y 2>&1 | tee -a "$REPORT" + +log "" +log "[Azas team bootstrap] building workspace" +colcon build --symlink-install 2>&1 | tee -a "$REPORT" + +check_file "$INSTALL_SETUP" +# shellcheck source=/dev/null +source "$INSTALL_SETUP" + +log "" +log "[Azas team bootstrap] verifying required packages" +missing=0 +for pkg in \ + azas_bringup \ + azas_dispenser \ + azas_gripper \ + azas_interfaces \ + azas_motion \ + azas_perception \ + azas_task_manager \ + azas_voice \ + dsr_bringup2 \ + dsr_msgs2 \ + dsr_moveit_config_m0609 \ + realsense2_camera; do + check_ros_pkg "$pkg" || missing=1 +done + +log "" +log "[Azas team bootstrap] verifying panel entrypoint" +check_file "$ROOT/tools/run/open_robot_pipeline_control_panel.sh" || missing=1 +check_file "$ROOT/tools/run/robot_pipeline_control_server.py" || missing=1 +check_file "$ROOT/docs/robot_pipeline_control.html" || missing=1 + +log "" +log "[Azas team bootstrap] verifying YOLO model link" +if [[ ! -f "$ROOT/local_models/best.pt" ]]; then + if [[ -f "/home/ssu/Downloads/best.pt" ]]; then + "$ROOT/tools/setup/link_yolo_model.sh" "/home/ssu/Downloads/best.pt" 2>&1 | tee -a "$REPORT" + else + log "[FAIL] missing YOLO model: $ROOT/local_models/best.pt" + log " Put best.pt on this PC, then run:" + log " bash tools/setup/link_yolo_model.sh /path/to/best.pt" + missing=1 + fi +else + log "[OK] YOLO model: $ROOT/local_models/best.pt" +fi + +if [[ "$missing" != "0" ]]; then + log "" + log "[FAIL] bootstrap completed but required packages/files are missing." + log " See report: $REPORT" + exit 1 +fi + +log "" +log "[PASS] team PC bootstrap completed." +log "Next:" +log " cd $ROOT" +log " bash tools/run/open_robot_pipeline_control_panel.sh" +log "" +log "Robot connection fields must still match that PC/network:" +log " ROBOT_HOST=" +log " RT_HOST=" +log " SERVICE_PREFIX=dsr01" diff --git a/tools/setup/link_yolo_model.sh b/tools/setup/link_yolo_model.sh new file mode 100755 index 0000000..0d8ffac --- /dev/null +++ b/tools/setup/link_yolo_model.sh @@ -0,0 +1,36 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_PATH="$(readlink -f "${BASH_SOURCE[0]}")" +SCRIPT_DIR="$(cd "$(dirname "$SCRIPT_PATH")" && pwd)" +ROOT="$(cd "$SCRIPT_DIR/../.." && pwd)" +SOURCE_MODEL="${1:-${MODEL_PATH:-/home/ssu/Downloads/best.pt}}" +TARGET_DIR="$ROOT/local_models" +TARGET_MODEL="$TARGET_DIR/best.pt" + +mkdir -p "$TARGET_DIR" + +if [[ ! -f "$SOURCE_MODEL" ]]; then + cat >&2 < $(readlink -f "$TARGET_MODEL")" + +python3 - < 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 new file mode 100755 index 0000000..aaa7201 --- /dev/null +++ b/tools/smoke/smoke_one_click_cocktail_no_motion.sh @@ -0,0 +1,167 @@ +#!/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] PRESS_PRE measured press pre-contact joints: movej_deg=[-4.5, 26.0, 74.2, 146.7, 10.7, -142.0] +[Azas] PRESS_CONTACT measured contact joints 1/2: movej_deg=[10.8, 30.9, 57.9, -38.2, 25.1, 32.9] +[Azas] PRESS_Z_OVERDRIVE from measured contact 1/2: posx=[1,2,3,4,5,6] +[Azas] return to PRESS_CONTACT after Z overdrive 1/2: posx=[1,2,3,4,5,6] +[Azas] PRESS_CONTACT measured contact joints 2/2: movej_deg=[10.8, 30.9, 57.9, -38.2, 25.1, 32.9] +[Azas] PRESS_Z_OVERDRIVE from measured contact 2/2: posx=[1,2,3,4,5,6] +[Azas] return to PRESS_CONTACT after Z overdrive 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 + +calibration = yaml.safe_load(Path('src/azas_bringup/config/calibration.yaml').read_text()) +for dispenser_id in ("1", "2", "3", "4"): + outlet = calibration['dispenser_outlets'][dispenser_id] + for key in ( + 'cup_pre_place_joints_deg', + 'cup_place_joints_deg', + 'press_pre_joints_deg', + 'press_contact_joints_deg', + ): + actual = outlet[key] + assert len(actual) == 6, (dispenser_id, key, actual) + assert all(isinstance(float(value), float) for value in actual), (dispenser_id, key, actual) + +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 (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') +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" 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)) 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())