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/perception/human_hand_detection_node.py b/tools/perception/human_hand_detection_node.py index 699dde7..f514044 100755 --- a/tools/perception/human_hand_detection_node.py +++ b/tools/perception/human_hand_detection_node.py @@ -30,6 +30,7 @@ import rclpy from geometry_msgs.msg import PointStamped from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import CameraInfo, Image from std_msgs.msg import String @@ -47,9 +48,17 @@ WRIST = 0 PALM_LANDMARKS = (0, 5, 9, 13, 17) +DEPTH_FALLBACK_LANDMARKS = (0, 5, 9, 13, 17, 1, 2) FINGER_TIPS = (8, 12, 16, 20) FINGER_PIPS = (6, 10, 14, 18) +LOW_LATENCY_IMAGE_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, +) + # cv_bridge is avoided on purpose: the ROS humble build is ABI-incompatible # with the pip-installed numpy 2.x that mediapipe requires. @@ -100,17 +109,21 @@ def __init__(self, args: argparse.Namespace) -> None: ) self.landmarker = mp_vision.HandLandmarker.create_from_options(options) - self.point_pub = self.create_publisher(PointStamped, OUTPUT_TOPIC, 10) + self.point_pub = self.create_publisher(PointStamped, OUTPUT_TOPIC, LOW_LATENCY_IMAGE_QOS) 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.overlay_pub = ( + self.create_publisher(Image, OVERLAY_TOPIC, LOW_LATENCY_IMAGE_QOS) + 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.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, self.on_camera_info, LOW_LATENCY_IMAGE_QOS) + self.create_subscription(Image, DEPTH_TOPIC, self.on_depth, LOW_LATENCY_IMAGE_QOS) + self.create_subscription(Image, COLOR_TOPIC, self.on_color, LOW_LATENCY_IMAGE_QOS) self.get_logger().info( "human hand detection ready (perception-only, no motion commands). " f"publishing stable open-hand target on {OUTPUT_TOPIC}; " + f"processing width <= {args.process_width_px}px; " f"stability: {args.stable_min_samples} samples within {args.stable_radius_m:.3f}m " f"over >= {args.stable_min_seconds:.2f}s" ) @@ -132,7 +145,8 @@ def on_color(self, msg: Image) -> None: return color = image_msg_to_array(msg) - rgb = cv2.cvtColor(color, cv2.COLOR_BGR2RGB) + inference_bgr = self.resize_for_inference(color) + rgb = cv2.cvtColor(inference_bgr, 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) @@ -154,7 +168,7 @@ def on_color(self, msg: Image) -> None: 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) + depth_m, depth_px, depth_source = self.find_hand_depth_m(palm_px, pixels) status.update( { "detected": True, @@ -162,12 +176,16 @@ def on_color(self, msg: Image) -> None: "hand_open": hand_open, "palm_px": list(palm_px), "depth_m": None if depth_m is None else round(depth_m, 4), + "depth_px": None if depth_px is None else list(depth_px), + "depth_source": depth_source, } ) 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 depth_px is not None and depth_px != palm_px: + cv2.circle(overlay, depth_px, 6, (0, 255, 255), 2) if not hand_open: self.recent.clear() @@ -191,15 +209,32 @@ def on_color(self, msg: Image) -> None: if not stable: return + self.publish_status(status) point = PointStamped() - point.header.stamp = msg.header.stamp + point.header.stamp = self.get_clock().now().to_msg() 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: - self.overlay_pub.publish(bgr_array_to_image_msg(overlay, msg.header)) + self.overlay_pub.publish(bgr_array_to_image_msg(self.resize_overlay_for_publish(overlay), msg.header)) + + def resize_for_inference(self, color: np.ndarray) -> np.ndarray: + target_width = int(self.args.process_width_px) + height, width = color.shape[:2] + if target_width <= 0 or width <= target_width: + return color + target_height = max(1, int(round(height * (target_width / width)))) + return cv2.resize(color, (target_width, target_height), interpolation=cv2.INTER_AREA) + + def resize_overlay_for_publish(self, overlay: np.ndarray) -> np.ndarray: + target_width = int(self.args.overlay_width_px) + height, width = overlay.shape[:2] + if target_width <= 0 or width <= target_width: + return overlay + target_height = max(1, int(round(height * (target_width / width)))) + return cv2.resize(overlay, (target_width, target_height), interpolation=cv2.INTER_AREA) 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.""" @@ -212,11 +247,45 @@ def count_extended_fingers(self, pixels: list[tuple[float, float]]) -> int: count += 1 return count - def median_depth_m(self, palm_px: tuple[int, int]) -> float | None: + def find_hand_depth_m( + self, + palm_px: tuple[int, int], + pixels: list[tuple[float, float]], + ) -> tuple[float | None, tuple[int, int] | None, str]: + height, width = self.latest_depth.shape[:2] + candidates: list[tuple[str, tuple[int, int]]] = [("palm", palm_px)] + for idx in DEPTH_FALLBACK_LANDMARKS: + px = ( + int(np.clip(pixels[idx][0], 0, width - 1)), + int(np.clip(pixels[idx][1], 0, height - 1)), + ) + if px not in [item[1] for item in candidates]: + candidates.append((f"landmark_{idx}", px)) + + base_window = max(int(self.args.depth_window_px), 3) + max_window = max(int(self.args.max_depth_window_px), base_window) + window_sizes = [] + size = base_window + while size <= max_window: + window_sizes.append(size) + size *= 2 + if window_sizes[-1] != max_window: + window_sizes.append(max_window) + + for window_px in window_sizes: + for source, px in candidates: + depth_m = self.median_depth_m(px, window_px) + if depth_m is not None: + return depth_m, px, f"{source}@{window_px}px" + return None, None, "none" + + def median_depth_m(self, palm_px: tuple[int, int], window_px: int | None = None) -> float | None: depth = self.latest_depth if depth is None: return None - half = max(int(self.args.depth_window_px) // 2, 1) + if window_px is None: + window_px = int(self.args.depth_window_px) + half = max(int(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) @@ -258,11 +327,17 @@ def main() -> int: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("--model-path", default=DEFAULT_MODEL_PATH) parser.add_argument("--max-rate-hz", type=float, default=15.0) + parser.add_argument("--process-width-px", type=int, default=640, + help="resize color frames to this width for MediaPipe inference; <=0 disables resizing") + parser.add_argument("--overlay-width-px", type=int, default=640, + help="resize published overlay to this width for lower-latency viewing; <=0 keeps original") 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("--max-depth-window-px", type=int, default=63, + help="when palm depth is missing, retry larger windows and nearby hand landmarks") 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, diff --git a/tools/run/auto_handover_on_palm.py b/tools/run/auto_handover_on_palm.py index 1f04164..071c810 100755 --- a/tools/run/auto_handover_on_palm.py +++ b/tools/run/auto_handover_on_palm.py @@ -23,6 +23,8 @@ from __future__ import annotations import argparse +import json +import os import subprocess import sys import time @@ -30,21 +32,59 @@ ROOT = Path(__file__).resolve().parents[2] HANDOVER_SCRIPT = ROOT / "tools" / "run" / "handover_cup_to_palm.py" +DIRECT_MOVEJ = ROOT / "tools" / "run" / "direct_movej_joints.py" HAND_TOPIC = "/azas/human_hand_detection" +STATUS_TOPIC = "/azas/human_hand_detection/status" CONFIRM_PHRASE = "AUTO_HANDOVER_ON_PALM" +MOVEJ_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEJ" def wait_for_stable_palm(args: argparse.Namespace) -> bool: """Spin a perception-only node until the palm trigger fires or we time out.""" import rclpy from geometry_msgs.msg import PointStamped + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + from std_msgs.msg import String stamps: list[float] = [] + latest_status = {"detected": False, "reason": "no status yet"} + point_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + + def status_ready(status: dict[str, object]) -> bool: + depth_m = status.get("depth_m") + if depth_m is None: + return False + depth = float(depth_m) + return ( + bool(status.get("detected")) + and bool(status.get("hand_open")) + and bool(status.get("stable")) + and args.trigger_min_depth_m <= depth <= args.trigger_max_depth_m + and int(status.get("open_fingers", 0)) >= args.min_trigger_open_fingers + ) + + def on_status(msg: String) -> None: + nonlocal latest_status + try: + latest_status = json.loads(msg.data) + except json.JSONDecodeError: + latest_status = {"raw": msg.data} + if not status_ready(latest_status): + stamps.clear() + + def on_hand(_msg: PointStamped) -> None: + if status_ready(latest_status): + stamps.append(time.monotonic()) + rclpy.init() node = rclpy.create_node("azas_auto_handover_watch") - node.create_subscription( - PointStamped, HAND_TOPIC, lambda _msg: stamps.append(time.monotonic()), 10 - ) + node.create_subscription(PointStamped, HAND_TOPIC, on_hand, point_qos) + node.create_subscription(String, STATUS_TOPIC, on_status, 10) print( f"[Azas] 손 대기 시작: {args.trigger_window_sec:.1f}초 안에 안정 검출 " f"{args.trigger_stable_count}개가 쌓이면 핸드오버를 1회 실행합니다 " @@ -58,7 +98,8 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: rclpy.spin_once(node, timeout_sec=0.2) now = time.monotonic() stamps[:] = [t for t in stamps if now - t <= args.trigger_window_sec] - if len(stamps) >= args.trigger_stable_count: + stable_duration = (now - stamps[0]) if stamps else 0.0 + if len(stamps) >= args.trigger_stable_count and stable_duration >= args.trigger_min_stable_sec: triggered = True break if now - last_report >= 5.0: @@ -66,8 +107,9 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: remain = deadline - now print( f"[Azas] 대기 중... 최근 {args.trigger_window_sec:.1f}초 안정 검출 " - f"{len(stamps)}/{args.trigger_stable_count}개 (남은 시간 {remain:.0f}초). " - "손바닥을 펴고 정지해 주세요." + f"{len(stamps)}/{args.trigger_stable_count}개, 지속 {stable_duration:.1f}/" + f"{args.trigger_min_stable_sec:.1f}초 (남은 시간 {remain:.0f}초). " + f"status={latest_status}. 손바닥을 펴고 정지해 주세요." ) finally: node.destroy_node() @@ -76,15 +118,146 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: return triggered +def parse_joint_csv(value: str) -> list[float]: + joints = [float(part.strip()) for part in str(value).split(",") if part.strip()] + if len(joints) != 6: + raise ValueError(f"--observe-joints must contain 6 comma-separated values, got {len(joints)}") + return joints + + +def move_to_observe(args: argparse.Namespace) -> int: + joints = parse_joint_csv(args.observe_joints) + cmd = [ + sys.executable, str(DIRECT_MOVEJ), + "--service-prefix", args.service_prefix, + "--j1", f"{joints[0]:.6f}", + "--j2", f"{joints[1]:.6f}", + "--j3", f"{joints[2]:.6f}", + "--j4", f"{joints[3]:.6f}", + "--j5", f"{joints[4]:.6f}", + "--j6", f"{joints[5]:.6f}", + "--velocity", f"{args.observe_velocity:.3f}", + "--acceleration", f"{args.observe_acceleration:.3f}", + "--timeout-sec", f"{args.observe_timeout_sec:.1f}", + "--motion-timeout-sec", f"{args.observe_motion_timeout_sec:.1f}", + "--j5-min-deg", f"{args.observe_j5_min_deg:.3f}", + "--j5-max-deg", f"{args.observe_j5_max_deg:.3f}", + ] + if args.execute: + cmd += ["--execute", "--confirm", MOVEJ_CONFIRM_PHRASE] + print( + "[Azas] observe 위치로 먼저 이동합니다: joints_deg=[" + + ", ".join(f"{value:.1f}" for value in joints) + + f"] vel={args.observe_velocity:.1f}" + ) + rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode + if rc == 0: + print("[PASS] observe 위치 이동 완료.") + else: + print(f"[FAIL] observe 위치 이동 실패(rc={rc}); 손 대기/핸드오버를 시작하지 않습니다.") + return rc + + def main() -> int: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--service-prefix", default=os.environ.get("SERVICE_PREFIX", "")) + parser.add_argument("--no-service-prefix-fallback", action="store_true", + help="use exactly --service-prefix in the handover script; do not fall back to dsr01") parser.add_argument("--trigger-stable-count", type=int, default=12, help="trigger when this many stable detections land inside the window") parser.add_argument("--trigger-window-sec", type=float, default=3.0) + parser.add_argument("--trigger-min-stable-sec", type=float, default=0.0, + help="also require the stable detections to span at least this many seconds") + parser.add_argument("--min-trigger-open-fingers", type=int, default=3, + help="auto trigger also requires latest status.open_fingers >= this value") + parser.add_argument("--trigger-min-depth-m", type=float, default=0.30, + help="auto trigger requires latest palm depth to be at least this close/far range") + parser.add_argument("--trigger-max-depth-m", type=float, default=0.75, + help="auto trigger rejects hands farther than this camera depth") parser.add_argument("--wait-timeout-sec", type=float, default=180.0, help="give up (exit 3, no motion) when no stable palm appears in time") parser.add_argument("--release-tcp-above-palm-m", default="0.08") + parser.add_argument("--skip-observe", action="store_true", + help="do not move to the observe/camera-home joint pose before waiting for a palm") + parser.add_argument("--observe-joints", default="3.0,-12.7,44.0,-9.0,133.0,90.0", + help="comma-separated J1..J6 degrees for the initial observe/camera-home pose") + parser.add_argument("--observe-velocity", type=float, default=45.0) + parser.add_argument("--observe-acceleration", type=float, default=60.0) + parser.add_argument("--observe-timeout-sec", type=float, default=20.0) + parser.add_argument("--observe-motion-timeout-sec", type=float, default=120.0) + parser.add_argument("--observe-j5-min-deg", type=float, default=-150.0) + parser.add_argument("--observe-j5-max-deg", type=float, default=150.0) + parser.add_argument("--hand-sample-count", type=int, default=None) + parser.add_argument("--hand-sample-timeout-sec", type=float, default=None) + parser.add_argument("--hand-sample-spread-max-m", type=float, default=None) + parser.add_argument("--hand-recheck-tolerance-m", type=float, default=None) + parser.add_argument("--skip-hand-recheck", action="store_true", + help="handover to the initially sampled palm without the pre-descent re-check") + parser.add_argument("--transit-velocity", type=float, default=75.0) + parser.add_argument("--transit-acceleration", type=float, default=95.0) + parser.add_argument("--descent-velocity", type=float, default=22.0) + parser.add_argument("--descent-acceleration", type=float, default=32.0) + parser.add_argument("--descent-step-m", type=float, default=0.03) + parser.add_argument("--max-descent-steps", type=int, default=0, + help="maximum staged descent steps; 0 means use the Z floor only") + parser.add_argument("--force-search-start-above-palm-m", type=float, default=0.16, + help="with contact release, start force-only descent this far above the detected palm") + parser.add_argument("--force-search-below-palm-m", type=float, default=0.10, + help="with contact release, search down to this far below the detected palm before aborting") + parser.add_argument("--move-timeout-sec", type=float, default=None) + parser.add_argument("--verify-timeout-sec", type=float, default=None) + parser.add_argument("--target-tolerance-mm", type=float, default=None) + parser.add_argument("--ikin-timeout-sec", type=float, default=None) + parser.add_argument("--ikin-retries", type=int, default=None) + parser.add_argument("--ikin-sol-spaces", default=None) + parser.add_argument("--j5-min-deg", type=float, default=None) + parser.add_argument("--j5-max-deg", type=float, default=None) + parser.add_argument("--skip-force-monitor", action="store_true", + help="pass through to handover script; staged descent remains but force abort is disabled") + parser.add_argument("--force-abort-delta-n", type=float, default=2.0, + help="force rise over baseline that counts as palm contact during descent") + parser.add_argument("--force-axis-delta-n", type=float, default=1.0, + help="also count contact when any single force axis changes by this much") + parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="z", + help="force axes used for contact release; z is safest for vertical handover") + parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="positive", + help="when --contact-axis z, require this signed Z force delta for contact") + parser.add_argument("--contact-step-delta-n", type=float, default=2.0, + help="contact candidate also requires this force jump from the previous descent step") + parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=True, + help="also require total force magnitude to rise before contact release") + parser.add_argument("--force-magnitude-delta-n", type=float, default=1.5, + help="minimum total force magnitude rise required with --require-force-magnitude-delta") + parser.add_argument("--force-baseline-samples", type=int, default=5, + help="average this many GetToolForce samples before descent") + parser.add_argument("--force-baseline-interval-sec", type=float, default=0.05, + help="delay between baseline force samples") + parser.add_argument("--force-read-settle-sec", type=float, default=0.15, + help="wait after each descent step before reading force") + parser.add_argument("--release-on-contact", action=argparse.BooleanOptionalAction, default=True, + help="open the gripper at the first force/contact trigger during descent") + parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True, + help="with --release-on-contact, retreat with the cup if contact is never detected") + parser.add_argument("--contact-confirm-samples", type=int, default=5, + help="consecutive above-threshold force samples required before opening RG2") + parser.add_argument("--contact-confirm-min-hits", type=int, default=0, + help="minimum hit samples needed within --contact-confirm-samples; " + "0 means all samples") + parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.12, + help="delay between force confirmation samples") + parser.add_argument("--contact-relief-lift-m", type=float, default=0.0, + help="deprecated/ignored: contact release now opens RG2 at the confirmed contact pose") + parser.add_argument("--contact-search-below-release-m", type=float, default=0.20, + help="with --release-on-contact, keep descending this far below release height while seeking contact") + parser.add_argument("--gripper-open-retries", type=int, default=None) + parser.add_argument("--gripper-open-retry-sleep-sec", type=float, default=None) + parser.add_argument("--x-min", type=float, default=None) + parser.add_argument("--x-max", type=float, default=None) + parser.add_argument("--y-min", type=float, default=None) + parser.add_argument("--y-max", type=float, default=None) + parser.add_argument("--z-min", type=float, default=None) + parser.add_argument("--z-max", type=float, default=None) + parser.add_argument("--palm-z-max-m", type=float, default=None) parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} with --execute") args = parser.parse_args() @@ -92,9 +265,17 @@ def main() -> int: if args.execute and args.confirm != CONFIRM_PHRASE: print(f"[BLOCKED] --execute requires --confirm {CONFIRM_PHRASE}") return 2 + if args.release_on_contact and args.skip_force_monitor: + print("[BLOCKED] contact-release mode requires force monitoring; remove --skip-force-monitor") + return 2 if not args.execute: print("[DRY-RUN] --execute 미지정: 손 트리거 후 핸드오버도 dry-run(인식+계획만)으로 실행합니다.") + if not args.skip_observe: + rc = move_to_observe(args) + if rc != 0: + return rc + if not wait_for_stable_palm(args): print( f"[FAIL] {args.wait_timeout_sec:.0f}초 안에 안정적인 손바닥이 없어 종료합니다 (로봇 모션 없음). " @@ -107,16 +288,85 @@ def main() -> int: sys.executable, str(HANDOVER_SCRIPT), "--service-prefix", args.service_prefix, "--release-tcp-above-palm-m", str(args.release_tcp_above_palm_m), - "--transit-velocity", "10.0", "--transit-acceleration", "14.0", - "--descent-velocity", "4.0", "--descent-acceleration", "6.0", - "--force-abort-delta-n", "10.0", + "--transit-velocity", f"{args.transit_velocity:.3f}", + "--transit-acceleration", f"{args.transit_acceleration:.3f}", + "--descent-velocity", f"{args.descent_velocity:.3f}", + "--descent-acceleration", f"{args.descent_acceleration:.3f}", + "--descent-step-m", f"{args.descent_step_m:.3f}", + "--max-descent-steps", str(args.max_descent_steps), + "--force-search-start-above-palm-m", f"{args.force_search_start_above_palm_m:.3f}", + "--force-search-below-palm-m", f"{args.force_search_below_palm_m:.3f}", + "--force-abort-delta-n", f"{args.force_abort_delta_n:.3f}", + "--force-axis-delta-n", f"{args.force_axis_delta_n:.3f}", + "--contact-axis", args.contact_axis, + "--contact-z-direction", args.contact_z_direction, + "--contact-step-delta-n", f"{args.contact_step_delta_n:.3f}", + "--force-magnitude-delta-n", f"{args.force_magnitude_delta_n:.3f}", + "--force-baseline-samples", str(args.force_baseline_samples), + "--force-baseline-interval-sec", f"{args.force_baseline_interval_sec:.3f}", + "--force-read-settle-sec", f"{args.force_read_settle_sec:.3f}", + "--contact-confirm-samples", str(args.contact_confirm_samples), + "--contact-confirm-min-hits", str(args.contact_confirm_min_hits), + "--contact-confirm-interval-sec", f"{args.contact_confirm_interval_sec:.3f}", + "--contact-relief-lift-m", f"{args.contact_relief_lift_m:.3f}", + "--contact-search-below-release-m", f"{args.contact_search_below_release_m:.3f}", ] + if args.hand_sample_count is not None: + cmd += ["--hand-sample-count", str(args.hand_sample_count)] + if args.hand_sample_timeout_sec is not None: + cmd += ["--hand-sample-timeout-sec", f"{args.hand_sample_timeout_sec:.1f}"] + if args.hand_sample_spread_max_m is not None: + cmd += ["--hand-sample-spread-max-m", f"{args.hand_sample_spread_max_m:.3f}"] + if args.hand_recheck_tolerance_m is not None: + cmd += ["--hand-recheck-tolerance-m", f"{args.hand_recheck_tolerance_m:.3f}"] + if args.skip_hand_recheck: + cmd += ["--skip-hand-recheck"] + if args.move_timeout_sec is not None: + cmd += ["--move-timeout-sec", f"{args.move_timeout_sec:.1f}"] + if args.verify_timeout_sec is not None: + cmd += ["--verify-timeout-sec", f"{args.verify_timeout_sec:.1f}"] + if args.target_tolerance_mm is not None: + cmd += ["--target-tolerance-mm", f"{args.target_tolerance_mm:.1f}"] + if args.ikin_timeout_sec is not None: + cmd += ["--ikin-timeout-sec", f"{args.ikin_timeout_sec:.1f}"] + if args.ikin_retries is not None: + cmd += ["--ikin-retries", str(args.ikin_retries)] + if args.ikin_sol_spaces: + cmd += ["--ikin-sol-spaces", args.ikin_sol_spaces] + if args.j5_min_deg is not None: + cmd += ["--j5-min-deg", f"{args.j5_min_deg:.3f}"] + if args.j5_max_deg is not None: + cmd += ["--j5-max-deg", f"{args.j5_max_deg:.3f}"] + if args.skip_force_monitor: + cmd += ["--skip-force-monitor"] + if args.release_on_contact: + cmd += ["--release-on-contact"] + if args.require_contact_for_release: + cmd += ["--require-contact-for-release"] + else: + cmd += ["--no-require-contact-for-release"] + if args.require_force_magnitude_delta: + cmd += ["--require-force-magnitude-delta"] + else: + cmd += ["--no-require-force-magnitude-delta"] + if args.gripper_open_retries is not None: + cmd += ["--gripper-open-retries", str(args.gripper_open_retries)] + if args.gripper_open_retry_sleep_sec is not None: + cmd += ["--gripper-open-retry-sleep-sec", f"{args.gripper_open_retry_sleep_sec:.1f}"] + for name in ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max"): + value = getattr(args, name) + if value is not None: + cmd += [f"--{name.replace('_', '-')}", f"{value:.3f}"] + if args.palm_z_max_m is not None: + cmd += ["--palm-z-max-m", f"{args.palm_z_max_m:.3f}"] if args.execute: cmd += [ "--execute", "--confirm", "ENABLE_HUMAN_PALM_HANDOVER", "--approve-motion", "ENABLE_HUMAN_PALM_HANDOVER_MOTION", "--approve-release", "RELEASE_CUP_NOW", ] + if args.no_service_prefix_fallback: + cmd += ["--no-service-prefix-fallback"] rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode if rc == 0: print("[PASS] 자동 핸드오버 완료.") diff --git a/tools/run/direct_movel_xyz.py b/tools/run/direct_movel_xyz.py index 4697f9a..7a2c8f2 100755 --- a/tools/run/direct_movel_xyz.py +++ b/tools/run/direct_movel_xyz.py @@ -14,7 +14,7 @@ from typing import Any import rclpy -from dsr_msgs2.srv import GetCurrentPosx, GetLastAlarm, Ikin, MoveLine +from dsr_msgs2.srv import CheckMotion, GetCurrentPosx, GetLastAlarm, Ikin, MoveJoint, MoveLine, MoveWait DR_BASE = 0 @@ -118,6 +118,22 @@ def xyz_distance_mm(actual: list[float], target: list[float]) -> float: return sum((actual[index] - target[index]) ** 2 for index in range(3)) ** 0.5 +def normalize_deg_180(value: float) -> float: + """Return the equivalent angle inside [-180, 180).""" + return ((float(value) + 180.0) % 360.0) - 180.0 + + +def parse_int_list(value: str) -> list[int]: + result: list[int] = [] + for part in str(value).split(","): + item = part.strip() + if item: + result.append(int(item)) + if not result: + raise ValueError("empty integer list") + return result + + def wait_for_target( node: Any, prefix: str, @@ -128,8 +144,15 @@ def wait_for_target( ) -> bool: deadline = time.monotonic() + max(timeout_sec, 0.1) last_line = "" + last_error = "" while time.monotonic() < deadline: - actual = current_posx(node, prefix, timeout_sec=5.0) + try: + actual = current_posx(node, prefix, timeout_sec=5.0) + except RuntimeError as exc: + last_error = str(exc) + print(f"[WARN] target verification GetCurrentPosx failed: {exc}; retrying") + time.sleep(1.0) + continue distance = xyz_distance_mm(actual, target_pos_mm_deg) last_line = ( "[Azas] verify xyz=" @@ -144,10 +167,115 @@ def wait_for_target( print("[FAIL] target verification timeout") if last_line: print(last_line) + if last_error: + print(f"[Azas] last verification error: {last_error}") print(last_alarm_text(node, prefix, timeout_sec=5.0)) return False +def check_target_once( + node: Any, + prefix: str, + target_pos_mm_deg: list[float], + *, + tolerance_mm: float, +) -> bool: + actual = current_posx(node, prefix, timeout_sec=5.0) + distance = xyz_distance_mm(actual, target_pos_mm_deg) + print( + "[Azas] verify xyz=" + f"[{actual[0]:.1f}, {actual[1]:.1f}, {actual[2]:.1f}] " + f"target=[{target_pos_mm_deg[0]:.1f}, {target_pos_mm_deg[1]:.1f}, {target_pos_mm_deg[2]:.1f}] " + f"distance={distance:.1f}mm tolerance={tolerance_mm:.1f}mm" + ) + return distance <= tolerance_mm + + +def wait_until_motion_done(node: Any, prefix: str, timeout_sec: float) -> tuple[bool, str]: + """Wait until the Doosan controller finishes the accepted MoveLine command.""" + timeout_sec = max(timeout_sec, 0.1) + move_wait_name = prefixed_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 = prefixed_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 run_movej_fallback( + node: Any, + prefix: str, + joints_deg: list[float], + *, + velocity: float, + acceleration: float, + wait_service_sec: float, + timeout_sec: float, +) -> bool: + service = prefixed_service(prefix, "motion/move_joint") + client = node.create_client(MoveJoint, service) + if not client.wait_for_service(timeout_sec=max(wait_service_sec, 0.1)): + print(f"[FAIL] MoveJoint fallback service not available: {service}") + return False + req = MoveJoint.Request() + req.pos = 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 + print( + "[Azas] MoveJoint fallback target joints_deg=[" + + ", ".join(f"{value:.1f}" for value in joints_deg) + + f"] vel={velocity:.1f} acc={acceleration:.1f}" + ) + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=max(timeout_sec, 0.1)) + if not future.done(): + print(f"[FAIL] MoveJoint fallback response timeout after {timeout_sec:.1f}s") + return False + if future.exception() is not None: + print(f"[FAIL] MoveJoint fallback exception: {future.exception()}") + return False + response = future.result() + if response is None or not response.success: + print("[FAIL] MoveJoint fallback returned success=false") + return False + print("[PASS] MoveJoint fallback accepted by service") + done, wait_output = wait_until_motion_done(node, prefix, timeout_sec=timeout_sec) + print(f"[Azas] MoveJoint fallback completion wait: {wait_output}") + return done + + def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser( description="Move directly to one supplied XYZ/RPY target via Doosan MoveLine." @@ -180,14 +308,37 @@ def parse_args() -> argparse.Namespace: 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-space", type=int, default=2, help="first solution space used by --precheck-ikin") + parser.add_argument( + "--ikin-sol-spaces", + default="", + help="comma-separated solution spaces to try for --precheck-ikin; defaults to --ikin-sol-space only", + ) 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( + "--fallback-movej-on-verify-fail", + action="store_true", + help="if MoveLine accepts but target verification fails, move to the accepted IK joint target with MoveJoint", + ) + parser.add_argument("--fallback-movej-velocity", type=float, default=25.0) + parser.add_argument("--fallback-movej-acceleration", type=float, default=35.0) parser.add_argument("--timeout-sec", type=float, default=10.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 MoveLine is accepted", + ) + parser.add_argument( + "--no-wait-motion", + action="store_true", + help="return/verify immediately after MoveLine is accepted", + ) parser.add_argument("--x-min", type=float, default=0.10) parser.add_argument("--x-max", type=float, default=0.70) parser.add_argument("--y-min", type=float, default=-0.45) @@ -268,42 +419,75 @@ def main() -> int: assert node is not None + accepted_ikin_joints = None if args.precheck_ikin: - response = None attempts = max(int(args.ikin_retries), 1) - for attempt in range(1, attempts + 1): - req = Ikin.Request() - req.pos = pos_mm_deg - req.sol_space = int(args.ikin_sol_space) - req.ref = DR_BASE - try: - response = call_service( - node, - Ikin, - prefixed_service(args.service_prefix, "motion/ikin"), - req, - timeout_sec=max(args.ikin_timeout_sec, 0.1), - label="Ikin", - ) - break - except RuntimeError as exc: - if attempt >= attempts: - raise - print(f"[WARN] Ikin attempt {attempt}/{attempts} failed: {exc}; retrying") - time.sleep(1.0) - if not response.success: - print("[FAIL] Ikin returned success=false") - 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): + sol_spaces = parse_int_list(args.ikin_sol_spaces) if args.ikin_sol_spaces else [int(args.ikin_sol_space)] + accepted_ikin = None + last_ikin_error = "" + for sol_space in sol_spaces: + response = None + for attempt in range(1, attempts + 1): + req = Ikin.Request() + req.pos = pos_mm_deg + req.sol_space = int(sol_space) + req.ref = DR_BASE + try: + response = call_service( + node, + Ikin, + prefixed_service(args.service_prefix, "motion/ikin"), + req, + timeout_sec=max(args.ikin_timeout_sec, 0.1), + label=f"Ikin(sol_space={sol_space})", + ) + break + except RuntimeError as exc: + last_ikin_error = str(exc) + if attempt >= attempts: + print(f"[WARN] Ikin sol_space={sol_space} failed after {attempts} attempts: {exc}") + else: + print(f"[WARN] Ikin sol_space={sol_space} attempt {attempt}/{attempts} failed: {exc}; retrying") + time.sleep(1.0) + if response is None: + continue + if not response.success: + print(f"[WARN] Ikin sol_space={sol_space} returned success=false") + continue + normalized_joints = [normalize_deg_180(float(value)) for value in response.conv_posj] + print( + f"[Azas] Ikin precheck success sol_space={sol_space}: joints_deg=[" + + ", ".join(f"{value:.1f}" for value in response.conv_posj) + + "]" + ) + if any(abs(float(raw) - norm) > 180.0 for raw, norm in zip(response.conv_posj, normalized_joints)): 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." + "[Azas] Ikin equivalent joints normalized_deg=[" + + ", ".join(f"{value:.1f}" for value in normalized_joints) + + "]" ) + if len(response.conv_posj) >= 5: + joint5 = normalized_joints[4] + if not float(args.j5_min_deg) <= joint5 <= float(args.j5_max_deg): + last_ikin_error = ( + 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_ikin_error}; trying next solution space") + continue + accepted_ikin = (sol_space, normalized_joints) + accepted_ikin_joints = normalized_joints + break + if accepted_ikin is None: + if last_ikin_error: + print(f"[BLOCKED] {last_ikin_error}; refusing MoveLine.") return 2 + print("[FAIL] Ikin did not return an acceptable solution") + return 1 + print( + f"[PASS] Ikin accepted sol_space={accepted_ikin[0]} " + f"normalized_joints_deg=[{', '.join(f'{value:.1f}' for value in accepted_ikin[1])}]" + ) client = node.create_client(MoveLine, move_service) if not client.wait_for_service(timeout_sec=max(args.wait_service_sec, 0.1)): @@ -346,14 +530,57 @@ def main() -> int: print("[FAIL] MoveLine returned success=false") return 1 print("[PASS] MoveLine accepted by service") - if args.verify_target and not wait_for_target( - node, - args.service_prefix, - pos_mm_deg, - tolerance_mm=max(args.target_tolerance_mm, 0.1), - timeout_sec=max(args.verify_timeout_sec, 0.1), - ): - return 1 + 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 + if args.verify_target: + tolerance_mm = max(args.target_tolerance_mm, 0.1) + if args.fallback_movej_on_verify_fail and accepted_ikin_joints is not None: + try: + reached = check_target_once( + node, + args.service_prefix, + pos_mm_deg, + tolerance_mm=tolerance_mm, + ) + except RuntimeError as exc: + print(f"[WARN] immediate target verification failed: {exc}") + reached = False + else: + reached = wait_for_target( + node, + args.service_prefix, + pos_mm_deg, + tolerance_mm=tolerance_mm, + timeout_sec=max(args.verify_timeout_sec, 0.1), + ) + if not reached: + if args.fallback_movej_on_verify_fail and accepted_ikin_joints is not None: + print("[WARN] MoveLine accepted but target is not reached; trying MoveJoint IK fallback now") + if run_movej_fallback( + node, + args.service_prefix, + accepted_ikin_joints, + velocity=float(args.fallback_movej_velocity), + acceleration=float(args.fallback_movej_acceleration), + wait_service_sec=float(args.wait_service_sec), + timeout_sec=float(args.motion_timeout_sec), + ) and wait_for_target( + node, + args.service_prefix, + pos_mm_deg, + tolerance_mm=tolerance_mm, + timeout_sec=max(args.verify_timeout_sec, 0.1), + ): + print("[PASS] target reached after MoveJoint IK fallback") + return 0 + return 1 return 0 except RuntimeError as exc: print(f"[FAIL] {exc}") diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py index 11c0fbd..c68796d 100755 --- a/tools/run/handover_cup_to_palm.py +++ b/tools/run/handover_cup_to_palm.py @@ -8,8 +8,8 @@ bash tools/run/run_human_hand_detection.sh) and transform the palm into base frame via live TF base_link->link_6 and the measured T_gripper2camera hand-eye calibration. - 2. PLAN compute LIFT -> ABOVE_HIGH -> ABOVE_PALM -> staged descent - -> RELEASE -> RETREAT, all with the CURRENT side-grip + 2. PLAN compute LIFT -> APPROACH -> ABOVE_PALM -> staged descent + -> RELEASE/CONTACT_RELEASE -> RETREAT, all with the CURRENT side-grip orientation preserved (--use-current-rpy on every MoveLine). 3. GATES default is dry-run. --execute needs --confirm, a typed operator approval before any motion, a hand re-check right @@ -49,6 +49,23 @@ DIRECT_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEL" +def prefixed_service(prefix: str, suffix: str) -> str: + clean = prefix.strip("/") + return f"/{clean}/{suffix}" if clean else f"/{suffix}" + + +def resolve_service_prefix(node, srv_type, requested_prefix: str, wait_sec: float, *, allow_fallback: bool) -> str: + requested = requested_prefix.strip("/") + if requested or not allow_fallback: + return requested + for candidate in ("", "dsr01"): + name = prefixed_service(candidate, "aux_control/get_current_posx") + client = node.create_client(srv_type, name) + if client.wait_for_service(timeout_sec=max(0.1, wait_sec)): + return candidate + return requested + + class HandoverPerception: """rclpy helpers: palm sampling, live TCP pose, tool force. No motion.""" @@ -57,6 +74,7 @@ def __init__(self, args: argparse.Namespace) -> None: import tf2_ros from dsr_msgs2.srv import GetCurrentPosx, GetToolForce from geometry_msgs.msg import PointStamped + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy self.args = args self.rclpy = rclpy @@ -64,11 +82,29 @@ def __init__(self, args: argparse.Namespace) -> None: self.node = rclpy.create_node("azas_handover_cup_to_palm") self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) - prefix = args.service_prefix - self.get_posx = self.node.create_client(GetCurrentPosx, f"/{prefix}/aux_control/get_current_posx") - self.get_tool_force = self.node.create_client(GetToolForce, f"/{prefix}/aux_control/get_tool_force") + prefix = resolve_service_prefix( + self.node, + GetCurrentPosx, + args.service_prefix, + min(self.args.wait_service_sec, 1.0), + allow_fallback=not args.no_service_prefix_fallback, + ) + self.args.service_prefix = prefix + print(f"[Azas] Doosan service prefix: {prefix or ''}") + self.get_posx = self.node.create_client( + GetCurrentPosx, prefixed_service(prefix, "aux_control/get_current_posx") + ) + self.get_tool_force = self.node.create_client( + GetToolForce, prefixed_service(prefix, "aux_control/get_tool_force") + ) self.hand_points: list[tuple[float, list[float]]] = [] - self.node.create_subscription(PointStamped, HAND_TOPIC, self._on_hand, 10) + hand_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + self.node.create_subscription(PointStamped, HAND_TOPIC, self._on_hand, hand_qos) self.gripper2cam = np.load(str(args.hand_eye_npy)).astype(float) if abs(self.gripper2cam[:3, 3]).max() > 10.0: self.gripper2cam[:3, 3] /= 1000.0 @@ -114,6 +150,17 @@ def tool_force_n(self) -> list[float]: raise RuntimeError("GetToolForce returned success=false") return [float(v) for v in list(response.tool_force)[:3]] + def averaged_tool_force_n(self, *, samples: int, interval_sec: float) -> list[float]: + count = max(int(samples), 1) + total = [0.0, 0.0, 0.0] + for index in range(count): + force = self.tool_force_n() + for axis in range(3): + total[axis] += force[axis] + if index + 1 < count: + time.sleep(max(interval_sec, 0.0)) + return [value / count for value in total] + def base_to_camera(self) -> np.ndarray: import rclpy.time @@ -178,31 +225,163 @@ def sample_palm_base(self, *, label: str) -> list[float]: return palm -def run_movel(args: argparse.Namespace, xyz_m: list[float], *, label: str, velocity: float, acceleration: float) -> None: +def run_movel( + args: argparse.Namespace, + xyz_m: list[float], + *, + label: str, + velocity: float, + acceleration: float, + rpy_deg: list[float], + fallback_movej: bool = True, +) -> None: cmd = [ sys.executable, str(DIRECT_MOVEL), "--service-prefix", args.service_prefix, "--x", f"{xyz_m[0]:.6f}", "--y", f"{xyz_m[1]:.6f}", "--z", f"{xyz_m[2]:.6f}", - "--use-current-rpy", + "--rx", f"{rpy_deg[0]:.6f}", "--ry", f"{rpy_deg[1]:.6f}", "--rz", f"{rpy_deg[2]:.6f}", "--velocity", f"{velocity:.3f}", "--acceleration", f"{acceleration:.3f}", "--timeout-sec", f"{args.move_timeout_sec:.1f}", + "--motion-timeout-sec", f"{args.move_timeout_sec:.1f}", "--wait-service-sec", f"{args.wait_service_sec:.1f}", + "--verify-timeout-sec", f"{args.verify_timeout_sec:.1f}", + "--target-tolerance-mm", f"{args.target_tolerance_mm:.1f}", + "--ikin-timeout-sec", f"{args.ikin_timeout_sec:.1f}", + "--ikin-retries", str(args.ikin_retries), + "--ikin-sol-spaces", args.ikin_sol_spaces, + "--j5-min-deg", f"{args.j5_min_deg:.3f}", + "--j5-max-deg", f"{args.j5_max_deg:.3f}", "--x-min", f"{args.x_min:.3f}", "--x-max", f"{args.x_max:.3f}", "--y-min", f"{args.y_min:.3f}", "--y-max", f"{args.y_max:.3f}", "--z-min", f"{args.z_min:.3f}", "--z-max", f"{args.z_max:.3f}", ] if args.execute: cmd += ["--precheck-ikin", "--verify-target", "--execute", "--confirm", DIRECT_CONFIRM_PHRASE] + if fallback_movej: + cmd += [ + "--fallback-movej-on-verify-fail", + "--fallback-movej-velocity", f"{min(max(velocity, 5.0), args.transit_velocity):.3f}", + "--fallback-movej-acceleration", f"{min(max(acceleration, 10.0), args.transit_acceleration):.3f}", + ] print(f"[Azas] MOVE {label}: xyz_m=[{xyz_m[0]:.3f}, {xyz_m[1]:.3f}, {xyz_m[2]:.3f}] vel={velocity:.1f}") rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode if rc != 0: raise RuntimeError(f"MoveLine step failed: {label} (rc={rc})") +def monitored_axis_indices(mode: str) -> list[int]: + if mode == "all": + return [0, 1, 2] + if mode == "xy": + return [0, 1] + if mode == "z": + return [2] + raise ValueError(f"unsupported contact axis mode: {mode}") + + +def force_contact_metrics( + force: list[float], + baseline: list[float], + baseline_mag: float, + *, + contact_axis: str, +) -> tuple[float, list[float], float]: + force_mag = math.sqrt(sum(v * v for v in force)) + axis_delta = [force[i] - baseline[i] for i in range(3)] + max_axis_delta = max(abs(axis_delta[i]) for i in monitored_axis_indices(contact_axis)) + mag_delta = force_mag - baseline_mag + return mag_delta, axis_delta, max_axis_delta + + +def contact_axis_hit(axis_delta: list[float], mag_delta: float, *, args: argparse.Namespace) -> bool: + if args.require_force_magnitude_delta and mag_delta < args.force_magnitude_delta_n: + return False + if args.contact_axis == "z": + z_delta = axis_delta[2] + if args.contact_z_direction == "positive": + return z_delta > args.force_axis_delta_n + if args.contact_z_direction == "negative": + return z_delta < -args.force_axis_delta_n + return abs(z_delta) > args.force_axis_delta_n + if args.contact_axis == "xy": + return max(abs(axis_delta[0]), abs(axis_delta[1])) > args.force_axis_delta_n + return ( + max(abs(axis_delta[0]), abs(axis_delta[1]), abs(axis_delta[2])) > args.force_axis_delta_n + or mag_delta > args.force_abort_delta_n + ) + + +def contact_step_hit(force: list[float], previous_force: list[float], *, args: argparse.Namespace) -> bool: + step_delta = [force[i] - previous_force[i] for i in range(3)] + if args.contact_axis == "z": + z_delta = step_delta[2] + if args.contact_z_direction == "positive": + return z_delta > args.contact_step_delta_n + if args.contact_z_direction == "negative": + return z_delta < -args.contact_step_delta_n + return abs(z_delta) > args.contact_step_delta_n + if args.contact_axis == "xy": + return max(abs(step_delta[0]), abs(step_delta[1])) > args.contact_step_delta_n + return max(abs(step_delta[0]), abs(step_delta[1]), abs(step_delta[2])) > args.contact_step_delta_n + + +def force_contact_confirmed( + perception: HandoverPerception, + reference_force: list[float], + reference_mag: float, + *, + force_delta_n: float, + axis_delta_n: float, + contact_axis: str, + samples: int, + min_hits: int, + interval_sec: float, +) -> bool: + needed = max(int(samples), 1) + required_hits = min(max(int(min_hits), 1), needed) + hits = 0 + for index in range(needed): + force = perception.tool_force_n() + mag_delta, axis_delta, max_axis_delta = force_contact_metrics( + force, + reference_force, + reference_mag, + contact_axis=contact_axis, + ) + confirm_args = argparse.Namespace( + contact_axis=contact_axis, + contact_z_direction=perception.args.contact_z_direction, + force_axis_delta_n=axis_delta_n, + force_abort_delta_n=force_delta_n, + require_force_magnitude_delta=perception.args.require_force_magnitude_delta, + force_magnitude_delta_n=perception.args.force_magnitude_delta_n, + ) + hit = contact_axis_hit(axis_delta, mag_delta, args=confirm_args) + hits += 1 if hit else 0 + print( + "[Azas] contact confirm " + f"{index + 1}/{needed}: hit={hit} hits={hits}/{required_hits} " + f"fx={force[0]:.2f} fy={force[1]:.2f} fz={force[2]:.2f} " + f"delta_mag={mag_delta:.2f}N " + f"delta_axis=[{axis_delta[0]:.2f}, {axis_delta[1]:.2f}, {axis_delta[2]:.2f}] " + f"max_{contact_axis}_axis={max_axis_delta:.2f}N" + ) + if hits >= required_hits: + return True + remaining = needed - (index + 1) + if hits + remaining < required_hits: + return False + if index + 1 < needed: + time.sleep(max(interval_sec, 0.0)) + return hits >= required_hits + + def open_gripper(args: argparse.Namespace) -> None: env = os.environ.copy() env.setdefault("RG2_OPEN_TIMEOUT_SEC", "20.0") + env.setdefault("RG2_OPEN_RETRIES", str(args.gripper_open_retries)) + env.setdefault("RG2_OPEN_RETRY_SLEEP_SEC", f"{args.gripper_open_retry_sleep_sec:.1f}") rc = subprocess.run([str(RG2_OPEN)], cwd=str(ROOT), env=env, check=False).returncode if rc != 0: raise RuntimeError(f"RG2 open failed (rc={rc})") @@ -220,38 +399,96 @@ def require_typed_approval(phrase: str, *, prompt: str, preapproved: str = "") - def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--service-prefix", default=os.environ.get("SERVICE_PREFIX", "")) + parser.add_argument("--no-service-prefix-fallback", action="store_true", + help="use exactly --service-prefix, including empty prefix; do not fall back to dsr01") parser.add_argument("--hand-eye-npy", type=Path, default=DEFAULT_HAND_EYE) parser.add_argument("--hand-sample-count", type=int, default=10) parser.add_argument("--hand-sample-timeout-sec", type=float, default=20.0) parser.add_argument("--hand-sample-spread-max-m", type=float, default=0.03) parser.add_argument("--hand-recheck-tolerance-m", type=float, default=0.05, help="abort if the palm moved more than this between plan and descent") + parser.add_argument("--skip-hand-recheck", action="store_true", + help="use the initially sampled palm for descent without the pre-descent palm re-check") parser.add_argument("--transit-z-m", type=float, default=0.45) + parser.add_argument("--diagonal-approach", action=argparse.BooleanOptionalAction, default=True, + help="move toward the palm with XYZ blended to the descent-start height; " + "--no-diagonal-approach keeps the older XY-at-transit-height behavior") parser.add_argument("--above-palm-m", type=float, default=0.12, help="TCP height above the palm before the staged descent") parser.add_argument("--release-tcp-above-palm-m", type=float, default=0.08, help="TCP height above the palm at release. TUNE WITH A FOAM-BLOCK " "DRY TEST FIRST: depends on where the side grip holds the cup") - parser.add_argument("--descent-step-m", type=float, default=0.02) - parser.add_argument("--force-abort-delta-n", type=float, default=10.0, + parser.add_argument("--force-search-start-above-palm-m", type=float, default=0.16, + help="with --release-on-contact, start force-only descent this far above the detected palm") + parser.add_argument("--force-search-below-palm-m", type=float, default=0.10, + help="with --release-on-contact, search down to this far below the detected palm before aborting") + parser.add_argument("--descent-step-m", type=float, default=0.03) + parser.add_argument("--max-descent-steps", type=int, default=0, + help="maximum staged descent steps; 0 means use the Z floor only") + parser.add_argument("--force-abort-delta-n", type=float, default=2.0, help="abort descent when |tool force| rises this much over the pre-descent baseline") + parser.add_argument("--force-axis-delta-n", type=float, default=1.0, + help="trigger contact when the monitored force axis changes by this much") + parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="z", + help="force axes used for contact release; z is safest for vertical handover") + parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="positive", + help="when --contact-axis z, require this signed Z force delta for contact") + parser.add_argument("--contact-step-delta-n", type=float, default=2.0, + help="contact candidate also requires this force jump from the previous descent step") + parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=True, + help="also require total force magnitude to rise before contact release") + parser.add_argument("--force-magnitude-delta-n", type=float, default=1.5, + help="minimum total force magnitude rise required with --require-force-magnitude-delta") + parser.add_argument("--force-baseline-samples", type=int, default=5, + help="average this many GetToolForce samples before descent") + parser.add_argument("--force-baseline-interval-sec", type=float, default=0.05, + help="delay between baseline force samples") + parser.add_argument("--force-read-settle-sec", type=float, default=0.15, + help="wait after each descent step before reading force") + parser.add_argument("--release-on-contact", action="store_true", + help="during staged descent, treat a force rise as palm contact: stop, open RG2, then retreat") + parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True, + help="with --release-on-contact, only open RG2 after contact is detected") + parser.add_argument("--contact-confirm-samples", type=int, default=5, + help="consecutive above-threshold force samples required before opening RG2") + parser.add_argument("--contact-confirm-min-hits", type=int, default=0, + help="minimum hit samples needed within --contact-confirm-samples; " + "0 means all samples, preserving the strict default") + parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.12, + help="delay between force confirmation samples") + parser.add_argument("--contact-relief-lift-m", type=float, default=0.0, + help="deprecated/ignored: contact release now opens RG2 at the confirmed contact pose") + parser.add_argument("--contact-search-below-release-m", type=float, default=0.20, + help="with --release-on-contact, keep descending this far below release height while seeking contact") parser.add_argument("--retreat-lift-m", type=float, default=0.20) - parser.add_argument("--transit-velocity", type=float, default=10.0) - parser.add_argument("--transit-acceleration", type=float, default=14.0) - parser.add_argument("--descent-velocity", type=float, default=4.0) - parser.add_argument("--descent-acceleration", type=float, default=6.0) + parser.add_argument("--transit-velocity", type=float, default=75.0) + parser.add_argument("--transit-acceleration", type=float, default=95.0) + parser.add_argument("--descent-velocity", type=float, default=22.0) + parser.add_argument("--descent-acceleration", type=float, default=32.0) # Palm workspace bounds (base frame). The palm itself must be inside these. - parser.add_argument("--x-min", type=float, default=0.25) - parser.add_argument("--x-max", type=float, default=0.75) - parser.add_argument("--y-min", type=float, default=-0.45) - parser.add_argument("--y-max", type=float, default=0.45) - parser.add_argument("--z-min", type=float, default=0.05) - parser.add_argument("--z-max", type=float, default=0.60) - parser.add_argument("--palm-z-max-m", type=float, default=0.40, + parser.add_argument("--x-min", type=float, default=0.15) + parser.add_argument("--x-max", type=float, default=1.50) + parser.add_argument("--y-min", type=float, default=-0.65) + parser.add_argument("--y-max", type=float, default=0.75) + parser.add_argument("--z-min", type=float, default=0.04) + parser.add_argument("--z-max", type=float, default=0.75) + parser.add_argument("--palm-z-max-m", type=float, default=0.50, help="reject palms higher than this (likely a mis-detection)") parser.add_argument("--move-timeout-sec", type=float, default=60.0) + parser.add_argument("--verify-timeout-sec", type=float, default=90.0) + parser.add_argument("--target-tolerance-mm", type=float, default=25.0) + parser.add_argument("--ikin-timeout-sec", type=float, default=20.0) + parser.add_argument("--ikin-retries", type=int, default=2) + parser.add_argument("--ikin-sol-spaces", default="2,0,1,3,4,5,6,7", + help="solution spaces to try for every MoveLine IK precheck") + parser.add_argument("--j5-min-deg", type=float, default=-160.0) + parser.add_argument("--j5-max-deg", type=float, default=160.0) parser.add_argument("--wait-service-sec", type=float, default=10.0) + parser.add_argument("--skip-force-monitor", action="store_true", + help="skip GetToolForce monitoring during descent; keeps staged descent and release approval") + parser.add_argument("--gripper-open-retries", type=int, default=3) + parser.add_argument("--gripper-open-retry-sleep-sec", type=float, default=1.0) parser.add_argument("--auto-release", action="store_true", help="skip the final typed release approval (NOT recommended)") parser.add_argument("--test-hand-xyz-m", default="", @@ -277,6 +514,9 @@ def main() -> int: if args.release_tcp_above_palm_m >= args.above_palm_m: print("[BLOCKED] --release-tcp-above-palm-m must be below --above-palm-m") return 2 + if args.release_on_contact and args.skip_force_monitor: + print("[BLOCKED] --release-on-contact requires force monitoring; remove --skip-force-monitor") + return 2 if not args.execute: print("[DRY-RUN] --execute not set; perception + plan only, no robot command sent.") @@ -296,18 +536,47 @@ def main() -> int: return 1 lift = [current_m[0], current_m[1], max(current_m[2], args.transit_z_m)] - above_high = [palm[0], palm[1], max(args.transit_z_m, palm[2] + args.above_palm_m)] - above_palm = [palm[0], palm[1], palm[2] + args.above_palm_m] + contact_start_z = palm[2] + max(args.force_search_start_above_palm_m, 0.0) + descent_start_z = contact_start_z if args.release_on_contact else palm[2] + args.above_palm_m + if args.diagonal_approach: + approach_z = max(args.z_min, min(args.z_max, descent_start_z)) + approach_label = "APPROACH" + else: + approach_z = max(args.transit_z_m, descent_start_z) + approach_label = "ABOVE_HIGH" + approach = [palm[0], palm[1], approach_z] + above_palm = [ + palm[0], + palm[1], + descent_start_z, + ] release = [palm[0], palm[1], palm[2] + args.release_tcp_above_palm_m] + contact_floor_z = max(args.z_min, palm[2] - max(args.force_search_below_palm_m, 0.0)) retreat = [palm[0], palm[1], palm[2] + args.retreat_lift_m] - for name, pose in (("LIFT", lift), ("ABOVE_HIGH", above_high), ("ABOVE_PALM", above_palm), - ("RELEASE", release), ("RETREAT", retreat)): + plan_items = [("LIFT", lift), (approach_label, approach), ("ABOVE_PALM", above_palm)] + if not args.release_on_contact: + plan_items.append(("RELEASE", release)) + plan_items.append(("RETREAT", retreat)) + for name, pose in plan_items: print(f"[PLAN] {name}: xyz_m=[{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}]") + if args.release_on_contact: + print(f"[PLAN] CONTACT_SEARCH_FLOOR: z_m={contact_floor_z:.3f}") + print( + "[PLAN] force-only Z search: " + f"start_z={above_palm[2]:.3f} palm_z={palm[2]:.3f} floor_z={contact_floor_z:.3f}; " + "gripper opens only after confirmed contact" + ) print( - "[PLAN] descent ABOVE_PALM -> RELEASE in " - f"{math.ceil((above_palm[2] - release[2]) / max(args.descent_step_m, 0.005))} steps of " + "[PLAN] descent ABOVE_PALM -> " + f"{'CONTACT_SEARCH_FLOOR' if args.release_on_contact else 'RELEASE'} in " + f"{math.ceil((above_palm[2] - (contact_floor_z if args.release_on_contact else release[2])) / max(args.descent_step_m, 0.005))} steps of " f"{args.descent_step_m * 1000.0:.0f}mm with force abort delta {args.force_abort_delta_n:.1f}N" ) + if args.max_descent_steps > 0: + no_contact_action = "retreat with cup" if args.require_contact_for_release else "open at final descent pose" + print(f"[PLAN] max descent steps: {args.max_descent_steps} (no confirmed contact => {no_contact_action})") + if args.release_on_contact: + print("[PLAN] contact-release mode: keep descending until force/contact trigger, then open RG2") if not args.execute: return 0 @@ -323,48 +592,161 @@ def main() -> int: ), preapproved=args.approve_motion, ) + preserved_rpy = current[3:6] run_movel(args, lift, label="LIFT to transit height (Z-only)", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - run_movel(args, above_high, label="ABOVE_HIGH over palm at transit height", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - run_movel(args, above_palm, label="ABOVE_PALM vertical pre-descent", - velocity=args.descent_velocity, acceleration=args.descent_acceleration) - - # Hand must still be where we planned; people move. - recheck = perception.sample_palm_base(label="palm re-check before descent") - moved = math.dist(recheck, palm) - if moved > args.hand_recheck_tolerance_m: - print(f"[ABORT] palm moved {moved * 1000.0:.0f}mm since planning; retreating without descent") - run_movel(args, retreat, label="RETREAT after palm moved", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - return 1 + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + approach_motion_label = ( + "APPROACH to palm descent start (XYZ blended)" + if args.diagonal_approach else + "ABOVE_HIGH over palm at transit height" + ) + run_movel(args, approach, label=approach_motion_label, + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + if math.dist(approach, above_palm) > 0.001: + run_movel(args, above_palm, label="ABOVE_PALM vertical pre-descent", + velocity=args.descent_velocity, acceleration=args.descent_acceleration, + rpy_deg=preserved_rpy) + else: + print("[Azas] ABOVE_PALM equals approach target; skipping duplicate pre-descent move") - baseline = perception.tool_force_n() - baseline_mag = math.sqrt(sum(v * v for v in baseline)) + # Hand must still be where we planned; people move. This can be skipped + # when the camera re-check is known to jump after arm motion. + if args.skip_hand_recheck: + print("[Azas] palm re-check skipped; descending to the initially sampled palm target") + else: + recheck = perception.sample_palm_base(label="palm re-check before descent") + moved = math.dist(recheck, palm) + if moved > args.hand_recheck_tolerance_m: + print(f"[ABORT] palm moved {moved * 1000.0:.0f}mm since planning; retreating without descent") + run_movel(args, retreat, label="RETREAT after palm moved", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + + baseline = [0.0, 0.0, 0.0] + baseline_mag = 0.0 + if args.skip_force_monitor: + print("[Azas] force monitor skipped by operator option") + else: + baseline = perception.averaged_tool_force_n( + samples=args.force_baseline_samples, + interval_sec=args.force_baseline_interval_sec, + ) + baseline_mag = math.sqrt(sum(v * v for v in baseline)) + print( + "[Azas] force baseline: " + f"fx={baseline[0]:.2f} fy={baseline[1]:.2f} fz={baseline[2]:.2f} " + f"|f|={baseline_mag:.2f}N contact_axis={args.contact_axis} " + f"contact_z_direction={args.contact_z_direction}" + ) z = above_palm[2] - while z > release[2] + 1e-6: - z = max(z - max(args.descent_step_m, 0.005), release[2]) + contact_release = False + descent_floor_z = contact_floor_z if args.release_on_contact else release[2] + previous_force = list(baseline) + descent_step_index = 0 + while z > descent_floor_z + 1e-6: + if args.max_descent_steps > 0 and descent_step_index >= args.max_descent_steps: + print(f"[Azas] max descent steps reached ({args.max_descent_steps}) without confirmed contact") + break + descent_step_index += 1 + z = max(z - max(args.descent_step_m, 0.005), descent_floor_z) run_movel(args, [palm[0], palm[1], z], label=f"descent step to z={z:.3f}m", - velocity=args.descent_velocity, acceleration=args.descent_acceleration) - force = perception.tool_force_n() - force_mag = math.sqrt(sum(v * v for v in force)) - print(f"[Azas] tool force {force_mag:.1f}N (baseline {baseline_mag:.1f}N)") - if force_mag - baseline_mag > args.force_abort_delta_n: - print("[ABORT] force spike during descent (palm contact or obstruction); retreating with cup") - run_movel(args, retreat, label="RETREAT after force abort", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) + velocity=args.descent_velocity, acceleration=args.descent_acceleration, + rpy_deg=preserved_rpy) + if not args.skip_force_monitor: + time.sleep(max(args.force_read_settle_sec, 0.0)) + force = perception.tool_force_n() + force_mag = math.sqrt(sum(v * v for v in force)) + mag_delta, axis_delta, max_axis_delta = force_contact_metrics( + force, + baseline, + baseline_mag, + contact_axis=args.contact_axis, + ) + print( + "[Azas] tool force " + f"fx={force[0]:.2f} fy={force[1]:.2f} fz={force[2]:.2f} |f|={force_mag:.2f}N " + f"delta_mag={mag_delta:.2f}N " + f"delta_axis=[{axis_delta[0]:.2f}, {axis_delta[1]:.2f}, {axis_delta[2]:.2f}] " + f"step_delta=[{force[0] - previous_force[0]:.2f}, " + f"{force[1] - previous_force[1]:.2f}, {force[2] - previous_force[2]:.2f}] " + f"max_{args.contact_axis}_axis={max_axis_delta:.2f}N " + f"z_direction={args.contact_z_direction}" + ) + contact_candidate = ( + contact_axis_hit(axis_delta, mag_delta, args=args) + and contact_step_hit(force, previous_force, args=args) + ) + if contact_candidate: + if args.release_on_contact: + print("[Azas] contact candidate detected; checking confirmation samples before RG2 open") + candidate_reference_force = list(previous_force) + candidate_reference_mag = math.sqrt(sum(v * v for v in candidate_reference_force)) + if not force_contact_confirmed( + perception, + candidate_reference_force, + candidate_reference_mag, + force_delta_n=args.force_abort_delta_n, + axis_delta_n=args.force_axis_delta_n, + contact_axis=args.contact_axis, + samples=args.contact_confirm_samples, + min_hits=args.contact_confirm_min_hits or args.contact_confirm_samples, + interval_sec=args.contact_confirm_interval_sec, + ): + print( + "[Azas] contact candidate was not confirmed; " + "treating it as force noise and continuing descent" + ) + previous_force = force + continue + contact_release = True + print( + "[Azas] contact trigger during descent: " + f"delta_mag={mag_delta:.2f}N(limit {args.force_abort_delta_n:.2f}), " + f"max_{args.contact_axis}_axis={max_axis_delta:.2f}N(limit {args.force_axis_delta_n:.2f}); " + "opening RG2 at the contact candidate pose" + ) + break + print("[ABORT] force spike during descent (palm contact or obstruction); retreating with cup") + run_movel(args, retreat, label="RETREAT after force abort", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + previous_force = force + + if args.release_on_contact and not contact_release: + print("[Azas] contact search floor reached without contact trigger") + if args.require_contact_for_release: + print("[ABORT] contact was not detected; retreating with cup") + run_movel(args, retreat, label="RETREAT after no contact", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) return 1 + if args.release_on_contact and args.require_contact_for_release and not contact_release: + print("[ABORT] fail-closed: contact release was not confirmed; gripper will stay closed") + run_movel(args, retreat, label="RETREAT after unconfirmed contact release", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + if not args.auto_release: require_typed_approval( RELEASE_APPROVAL_PHRASE, - prompt="[Azas] Cup is at release height. Confirm the palm is directly under the cup.", + prompt=( + "[Azas] Contact detected; confirm the palm is supporting the cup." + if contact_release else + "[Azas] Cup is at release height. Confirm the palm is directly under the cup." + ), preapproved=args.approve_release, ) open_gripper(args) time.sleep(1.0) run_movel(args, retreat, label="RETREAT vertical after release", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) print("[PASS] palm handover sequence completed") return 0 except RuntimeError as exc: diff --git a/tools/run/rg2_full_open_verify.sh b/tools/run/rg2_full_open_verify.sh index 02bb56b..90e49c3 100755 --- a/tools/run/rg2_full_open_verify.sh +++ b/tools/run/rg2_full_open_verify.sh @@ -14,6 +14,8 @@ SERVICE="${RG2_SET_WIDTH_SERVICE:-/jarvis/rg2/set_width}" WIDTH_M="${RG2_FULL_OPEN_WIDTH_M:-0.110}" FORCE_N="${RG2_OPEN_FORCE_N:-25.0}" TIMEOUT_SEC="${RG2_OPEN_TIMEOUT_SEC:-12}" +RETRIES="${RG2_OPEN_RETRIES:-3}" +RETRY_SLEEP_SEC="${RG2_OPEN_RETRY_SLEEP_SEC:-1.0}" cd "${ROOT_DIR}" @@ -27,20 +29,30 @@ echo "[Azas] RG2 full-open request" echo "[Azas] service=${SERVICE}" echo "[Azas] command=open width_m=${WIDTH_M} force_n=${FORCE_N}" -if ! output="$( - python3 tools/run/rg2_set_width_verify.py \ - --service "${SERVICE}" \ - --command open \ - --width-m "${WIDTH_M}" \ - --force-n "${FORCE_N}" \ - --timeout-sec "${TIMEOUT_SEC}" \ - --ready-timeout-sec "${RG2_READY_TIMEOUT_SEC:-18}" \ - --rg2-ip "${RG2_IP:-192.168.1.1}" 2>&1 -)"; then +output="" +for attempt in $(seq 1 "${RETRIES}"); do + echo "[Azas] RG2 full-open attempt ${attempt}/${RETRIES}" + if output="$( + python3 tools/run/rg2_set_width_verify.py \ + --service "${SERVICE}" \ + --command open \ + --width-m "${WIDTH_M}" \ + --force-n "${FORCE_N}" \ + --timeout-sec "${TIMEOUT_SEC}" \ + --ready-timeout-sec "${RG2_READY_TIMEOUT_SEC:-18}" \ + --rg2-ip "${RG2_IP:-192.168.1.1}" 2>&1 + )"; then + break + fi echo "${output}" - echo "[FAIL] RG2 full-open service call failed" - exit 1 -fi + if [[ "${attempt}" -lt "${RETRIES}" ]]; then + echo "[WARN] RG2 full-open service call failed; retrying after ${RETRY_SLEEP_SEC}s" + sleep "${RETRY_SLEEP_SEC}" + else + echo "[FAIL] RG2 full-open service call failed after ${RETRIES} attempts" + exit 1 + fi +done echo "${output}" diff --git a/tools/run/run_human_hand_detection.sh b/tools/run/run_human_hand_detection.sh index d420978..8a4664d 100755 --- a/tools/run/run_human_hand_detection.sh +++ b/tools/run/run_human_hand_detection.sh @@ -13,7 +13,9 @@ if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then source "${ROOT_DIR}/install/setup.bash" fi -export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-15}" +export ROS_DOMAIN_ID="${AZAS_ROS_DOMAIN_ID:-${ROS_DOMAIN_ID:-15}}" export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +export MPLCONFIGDIR="${MPLCONFIGDIR:-/tmp/azas_mpl_config}" exec python3 "${ROOT_DIR}/tools/perception/human_hand_detection_node.py" "$@" diff --git a/tools/run/with_azas_ros_env.sh b/tools/run/with_azas_ros_env.sh new file mode 100755 index 0000000..51f82b0 --- /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="${ROS_LOCALHOST_ONLY:-0}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +export MPLCONFIGDIR="${MPLCONFIGDIR:-/tmp/azas_mpl_config}" + +exec "$@" diff --git a/tools/view/low_latency_image_view.py b/tools/view/low_latency_image_view.py new file mode 100755 index 0000000..9b13924 --- /dev/null +++ b/tools/view/low_latency_image_view.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +"""Low-latency ROS Image viewer. + +rqt_image_view is convenient, but in field testing it can appear to lag when +old image messages queue up. This viewer subscribes with BEST_EFFORT/depth=1 +and only displays the newest frame. +""" +from __future__ import annotations + +import argparse +import time + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import Image + + +LOW_LATENCY_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, +) + + +def image_msg_to_bgr(msg: Image) -> np.ndarray: + if msg.encoding == "bgr8": + return np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3).copy() + if msg.encoding == "rgb8": + rgb = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + if msg.encoding == "mono8": + mono = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width) + return cv2.cvtColor(mono, cv2.COLOR_GRAY2BGR) + if msg.encoding == "16UC1": + depth = np.frombuffer(msg.data, dtype=np.uint16).reshape(msg.height, msg.width) + normalized = cv2.normalize(depth, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + return cv2.cvtColor(normalized, cv2.COLOR_GRAY2BGR) + raise ValueError(f"unsupported image encoding: {msg.encoding}") + + +class LowLatencyImageView(Node): + def __init__(self, topic: str, window_name: str) -> None: + super().__init__("azas_low_latency_image_view") + self.topic = topic + self.window_name = window_name + self.latest: Image | None = None + self.last_fps_report = time.monotonic() + self.frames = 0 + self.create_subscription(Image, topic, self.on_image, LOW_LATENCY_QOS) + self.get_logger().info(f"viewing {topic} with BEST_EFFORT depth=1") + + def on_image(self, msg: Image) -> None: + self.latest = msg + + def show_once(self) -> bool: + if self.latest is None: + return True + msg = self.latest + self.latest = None + try: + frame = image_msg_to_bgr(msg) + except ValueError as exc: + self.get_logger().error(str(exc)) + return False + self.frames += 1 + now = time.monotonic() + if now - self.last_fps_report >= 1.0: + fps = self.frames / (now - self.last_fps_report) + self.frames = 0 + self.last_fps_report = now + cv2.setWindowTitle(self.window_name, f"{self.topic} {fps:.1f} fps") + cv2.imshow(self.window_name, frame) + return cv2.waitKey(1) not in (27, ord("q")) + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("topic", nargs="?", default="/azas/human_hand_detection/overlay") + parser.add_argument("--window-name", default="Azas Low Latency Image View") + args = parser.parse_args() + + rclpy.init() + node = LowLatencyImageView(args.topic, args.window_name) + cv2.namedWindow(args.window_name, cv2.WINDOW_NORMAL) + try: + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.001) + if not node.show_once(): + break + finally: + cv2.destroyAllWindows() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())