devagent-physical-engine 0.10.0__py3-none-any.whl

devagent_physical_engine/ros2/acceptance.py

@@ -0,0 +1,324 @@
+from __future__ import annotations
+
+from dataclasses import asdict, dataclass
+from datetime import datetime, timezone
+import json
+from pathlib import Path
+import os
+import re
+from time import monotonic, sleep
+from typing import Mapping
+
+from .commands import CommandResult, RosCommandRunner
+from .doctor import RosDoctor
+from .ur5e import UR5eSimulationLauncher, official_ur_motion_smoke_command
+
+
+@dataclass(frozen=True, slots=True)
+class AcceptanceStage:
+    name: str
+    passed: bool
+    detail: str
+    duration_s: float = 0.0
+
+
+@dataclass(frozen=True, slots=True)
+class LaptopAcceptanceReport:
+    started_at: str
+    robot: str
+    stages: tuple[AcceptanceStage, ...]
+    simulation_log: str
+
+    @property
+    def passed(self) -> bool:
+        return all(stage.passed for stage in self.stages)
+
+    def to_dict(self) -> dict:
+        return {
+            "started_at": self.started_at,
+            "robot": self.robot,
+            "passed": self.passed,
+            "simulation_log": self.simulation_log,
+            "stages": [asdict(stage) for stage in self.stages],
+        }
+
+    def write_json(self, path: Path) -> None:
+        target = path.expanduser().resolve()
+        target.parent.mkdir(parents=True, exist_ok=True)
+        target.write_text(json.dumps(self.to_dict(), indent=2) + "\n", encoding="utf-8")
+
+
+class UR5eLaptopAcceptance:
+    def __init__(
+        self,
+        runner: RosCommandRunner,
+        *,
+        ros_setup: Path | str = "/opt/ros/jazzy/setup.bash",
+        os_release_path: Path | str = "/etc/os-release",
+        environment: Mapping[str, str] | None = None,
+    ) -> None:
+        self.runner = runner
+        self.ros_setup = Path(ros_setup)
+        self.os_release_path = Path(os_release_path)
+        self.environment = dict(os.environ if environment is None else environment)
+
+    def _display_stage(self) -> AcceptanceStage:
+        display = self.environment.get("DISPLAY", "").strip()
+        wayland = self.environment.get("WAYLAND_DISPLAY", "").strip()
+        available = bool(display or wayland)
+        if display:
+            detail = f"DISPLAY={display}"
+        elif wayland:
+            detail = f"WAYLAND_DISPLAY={wayland}"
+        else:
+            detail = "no graphical display environment"
+        return AcceptanceStage("visual_display", available, detail)
+
+    def _joint_positions(self) -> tuple[float, ...]:
+        result = self.runner.run(
+            [
+                "ros2",
+                "topic",
+                "echo",
+                "/joint_states",
+                "--once",
+                "--field",
+                "position",
+            ],
+            timeout_s=8.0,
+        )
+        if not result.ok:
+            raise RuntimeError(result.stderr.strip() or "joint_state_snapshot_failed")
+        values = tuple(
+            float(match)
+            for match in re.findall(
+                r"[-+]?(?:\d+(?:\.\d*)?|\.\d+)(?:[eE][-+]?\d+)?",
+                result.stdout,
+            )
+        )
+        if len(values) < 6:
+            raise RuntimeError(f"joint_state_snapshot_invalid:{len(values)}")
+        return values[:6]
+
+    @staticmethod
+    def _motion_observed(
+        before: tuple[float, ...], after: tuple[float, ...]
+    ) -> tuple[bool, str]:
+        if len(before) != len(after):
+            return False, "joint_vector_size_changed"
+        deltas = tuple(abs(a - b) for a, b in zip(before, after))
+        max_delta = max(deltas, default=0.0)
+        return max_delta >= 0.01, f"max_joint_delta_rad={max_delta:.6f}"
+
+    def _ready_probe(self) -> tuple[bool, str]:
+        controllers = self.runner.run(
+            ["ros2", "control", "list_controllers"], timeout_s=5.0
+        )
+        if not controllers.ok:
+            return False, controllers.stderr.strip() or "controller_manager_unavailable"
+        controller_active = any(
+            "joint_trajectory_controller" in line.lower()
+            and line.split()
+            and line.split()[-1].lower() == "active"
+            for line in controllers.stdout.splitlines()
+        )
+        if not controller_active:
+            return False, "joint_trajectory_controller_not_active"
+
+        topics = self.runner.run(["ros2", "topic", "list"], timeout_s=5.0)
+        if not topics.ok:
+            return False, topics.stderr.strip() or "topic_list_failed"
+        if "/joint_states" not in {line.strip() for line in topics.stdout.splitlines()}:
+            return False, "joint_states_missing"
+        return True, "controller active; /joint_states present"
+
+    def _wait_ready(self, simulation, *, timeout_s: float) -> AcceptanceStage:
+        started = monotonic()
+        last_detail = "not_ready"
+        while monotonic() - started < timeout_s:
+            if not simulation.running:
+                return AcceptanceStage(
+                    "simulation_ready",
+                    False,
+                    "simulation_process_exited_early",
+                    monotonic() - started,
+                )
+            try:
+                ready, last_detail = self._ready_probe()
+            except Exception as exc:  # environment may still be starting
+                last_detail = f"probe_error:{type(exc).__name__}"
+                ready = False
+            if ready:
+                return AcceptanceStage(
+                    "simulation_ready",
+                    True,
+                    last_detail,
+                    monotonic() - started,
+                )
+            sleep(2.0)
+        return AcceptanceStage(
+            "simulation_ready",
+            False,
+            f"startup_timeout:{last_detail}",
+            monotonic() - started,
+        )
+
+    def run(
+        self,
+        *,
+        log_dir: Path,
+        startup_timeout_s: float = 120.0,
+        motion_timeout_s: float = 90.0,
+        keep_open: bool = False,
+    ) -> LaptopAcceptanceReport:
+        if startup_timeout_s <= 0 or motion_timeout_s <= 0:
+            raise ValueError("acceptance_timeouts_must_be_positive")
+
+        timestamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
+        root = log_dir.expanduser().resolve()
+        root.mkdir(parents=True, exist_ok=True)
+        sim_log = root / f"ur5e-gazebo-{timestamp}.log"
+        stages: list[AcceptanceStage] = []
+
+        display_stage = self._display_stage()
+        stages.append(display_stage)
+        if not display_stage.passed:
+            return LaptopAcceptanceReport(timestamp, "ur5e", tuple(stages), str(sim_log))
+
+        doctor_started = monotonic()
+        doctor = RosDoctor(
+            self.runner,
+            ros_setup=self.ros_setup,
+            os_release_path=self.os_release_path,
+        ).run()
+        stages.append(
+            AcceptanceStage(
+                "ros_doctor",
+                doctor.passed,
+                (
+                    "all required checks passed"
+                    if doctor.passed
+                    else "; ".join(
+                        f"{check.name}:{check.detail}" for check in doctor.failures
+                    )
+                ),
+                monotonic() - doctor_started,
+            )
+        )
+        if not doctor.passed:
+            return LaptopAcceptanceReport(timestamp, "ur5e", tuple(stages), str(sim_log))
+
+        launcher = UR5eSimulationLauncher(self.runner)
+        launch_started = monotonic()
+        simulation = launcher.launch(log_path=sim_log)
+        stages.append(
+            AcceptanceStage(
+                "simulation_launch",
+                simulation.running,
+                "process started",
+                monotonic() - launch_started,
+            )
+        )
+        if not simulation.running:
+            return LaptopAcceptanceReport(timestamp, "ur5e", tuple(stages), str(sim_log))
+
+        try:
+            ready = self._wait_ready(simulation, timeout_s=startup_timeout_s)
+            stages.append(ready)
+            if not ready.passed:
+                return LaptopAcceptanceReport(timestamp, "ur5e", tuple(stages), str(sim_log))
+
+            snapshot_started = monotonic()
+            before: tuple[float, ...] | None = None
+            try:
+                before = self._joint_positions()
+                stages.append(
+                    AcceptanceStage(
+                        "pre_motion_joint_state",
+                        True,
+                        "captured 6 joint positions",
+                        monotonic() - snapshot_started,
+                    )
+                )
+            except Exception as exc:
+                stages.append(
+                    AcceptanceStage(
+                        "pre_motion_joint_state",
+                        False,
+                        f"{type(exc).__name__}:{exc}",
+                        monotonic() - snapshot_started,
+                    )
+                )
+
+            if stages[-1].passed:
+                motion_started = monotonic()
+                try:
+                    motion: CommandResult = self.runner.run(
+                        official_ur_motion_smoke_command(),
+                        timeout_s=motion_timeout_s,
+                    )
+                    detail = (
+                        motion.stdout.strip()[-1000:]
+                        if motion.ok
+                        else (
+                            motion.stderr.strip()[-1000:] or "motion_smoke_failed"
+                        )
+                    )
+                    stages.append(
+                        AcceptanceStage(
+                            "ur5e_motion_smoke",
+                            motion.ok,
+                            detail,
+                            monotonic() - motion_started,
+                        )
+                    )
+                except Exception as exc:
+                    stages.append(
+                        AcceptanceStage(
+                            "ur5e_motion_smoke",
+                            False,
+                            f"{type(exc).__name__}:{exc}",
+                            monotonic() - motion_started,
+                        )
+                    )
+
+            if stages[-1].passed and before is not None:
+                observed_started = monotonic()
+                try:
+                    after = self._joint_positions()
+                    moved, motion_detail = self._motion_observed(before, after)
+                    stages.append(
+                        AcceptanceStage(
+                            "joint_motion_observed",
+                            moved,
+                            motion_detail,
+                            monotonic() - observed_started,
+                        )
+                    )
+                except Exception as exc:
+                    stages.append(
+                        AcceptanceStage(
+                            "joint_motion_observed",
+                            False,
+                            f"{type(exc).__name__}:{exc}",
+                            monotonic() - observed_started,
+                        )
+                    )
+
+            if stages[-1].passed:
+                probe_started = monotonic()
+                post_ok, post_detail = self._ready_probe()
+                stages.append(
+                    AcceptanceStage(
+                        "post_motion_health",
+                        post_ok,
+                        post_detail,
+                        monotonic() - probe_started,
+                    )
+                )
+        finally:
+            current_success = bool(stages) and all(stage.passed for stage in stages)
+            if not keep_open or not current_success:
+                simulation.stop()
+
+        return LaptopAcceptanceReport(timestamp, "ur5e", tuple(stages), str(sim_log))

devagent_physical_engine/ros2/commands.py

@@ -0,0 +1,175 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+import os
+import shlex
+import signal
+import subprocess
+from time import monotonic
+from typing import Protocol, Sequence, TextIO
+
+
+class CommandExecutionError(RuntimeError):
+    pass
+
+
+@dataclass(frozen=True, slots=True)
+class CommandResult:
+    command: tuple[str, ...]
+    returncode: int
+    stdout: str
+    stderr: str
+    duration_s: float
+
+    @property
+    def ok(self) -> bool:
+        return self.returncode == 0
+
+
+class RunningProcess(Protocol):
+    pid: int
+
+    def poll(self) -> int | None: ...
+    def wait(self, timeout: float | None = None) -> int: ...
+
+
+class RosCommandRunner(Protocol):
+    def run(self, command: Sequence[str], *, timeout_s: float = 15.0) -> CommandResult: ...
+    def start(self, command: Sequence[str], *, log_path: Path) -> RunningProcess: ...
+    def stop(self, process: RunningProcess, *, timeout_s: float = 10.0) -> None: ...
+
+
+def _validate_setup(path: Path) -> Path:
+    resolved = path.expanduser().resolve()
+    if not resolved.is_file():
+        raise FileNotFoundError(f"ros_setup_not_found:{resolved}")
+    return resolved
+
+
+def _clean_ros_environment() -> dict[str, str]:
+    """Return a ROS child environment isolated from Python virtual environments.
+
+    ROS executable scripts such as ``example_move.py`` commonly use
+    ``#!/usr/bin/env python3``. If DevAgent is launched from an activated virtualenv,
+    inheriting that PATH can make ROS load the virtualenv interpreter while importing
+    ROS modules from /opt/ros, which then misses distro-managed dependencies such as
+    PyYAML or NumPy. Keep desktop/session variables, but remove Python environment
+    overrides and the active virtualenv's bin directory before sourcing ROS.
+    """
+
+    environment = dict(os.environ)
+    virtual_env = environment.pop("VIRTUAL_ENV", "").strip()
+    environment.pop("PYTHONHOME", None)
+    environment.pop("PYTHONPATH", None)
+
+    path_entries = environment.get("PATH", "").split(os.pathsep)
+    if virtual_env:
+        virtual_bin = os.path.normpath(os.path.join(virtual_env, "bin"))
+        path_entries = [
+            entry
+            for entry in path_entries
+            if entry and os.path.normpath(entry) != virtual_bin
+        ]
+    environment["PATH"] = os.pathsep.join(path_entries)
+    return environment
+
+
+class SubprocessRosRunner:
+    """Run ROS commands in a sourced, virtualenv-isolated environment."""
+
+    def __init__(
+        self,
+        *,
+        ros_setup: Path | str = "/opt/ros/jazzy/setup.bash",
+        workspace_setup: Path | str | None = None,
+    ) -> None:
+        self.ros_setup = Path(ros_setup)
+        self.workspace_setup = Path(workspace_setup) if workspace_setup else None
+        self._logs: dict[int, TextIO] = {}
+
+    def _script(self, command: Sequence[str]) -> str:
+        if not command or any(not isinstance(part, str) or not part for part in command):
+            raise ValueError("command_parts_must_be_nonempty_strings")
+        setup = _validate_setup(self.ros_setup)
+        # ROS/ament setup scripts may legitimately reference variables before they are
+        # defined, so nounset (-u) is deliberately not enabled while sourcing them.
+        pieces = ["set -eo pipefail", f"source {shlex.quote(str(setup))}"]
+        if self.workspace_setup is not None:
+            workspace = _validate_setup(self.workspace_setup)
+            pieces.append(f"source {shlex.quote(str(workspace))}")
+        pieces.append("exec " + shlex.join(tuple(command)))
+        return "; ".join(pieces)
+
+    def run(self, command: Sequence[str], *, timeout_s: float = 15.0) -> CommandResult:
+        if timeout_s <= 0:
+            raise ValueError("timeout_s_must_be_positive")
+        script = self._script(command)
+        started = monotonic()
+        try:
+            completed = subprocess.run(
+                ["bash", "--noprofile", "--norc", "-lc", script],
+                capture_output=True,
+                text=True,
+                timeout=timeout_s,
+                check=False,
+                env=_clean_ros_environment(),
+            )
+        except subprocess.TimeoutExpired as exc:
+            raise CommandExecutionError(
+                f"command_timeout:{command[0]}:{timeout_s}s"
+            ) from exc
+        return CommandResult(
+            tuple(command),
+            completed.returncode,
+            completed.stdout,
+            completed.stderr,
+            monotonic() - started,
+        )
+
+    def start(self, command: Sequence[str], *, log_path: Path) -> subprocess.Popen[str]:
+        script = self._script(command)
+        path = log_path.expanduser().resolve()
+        path.parent.mkdir(parents=True, exist_ok=True)
+        log = path.open("a", encoding="utf-8", buffering=1)
+        try:
+            process = subprocess.Popen(
+                ["bash", "--noprofile", "--norc", "-lc", script],
+                stdout=log,
+                stderr=subprocess.STDOUT,
+                text=True,
+                start_new_session=True,
+                env=_clean_ros_environment(),
+            )
+        except Exception:
+            log.close()
+            raise
+        self._logs[process.pid] = log
+        return process
+
+    def stop(self, process: RunningProcess, *, timeout_s: float = 10.0) -> None:
+        try:
+            if process.poll() is None:
+                try:
+                    os.killpg(process.pid, signal.SIGINT)
+                except ProcessLookupError:
+                    pass
+                try:
+                    process.wait(timeout=timeout_s)
+                except subprocess.TimeoutExpired:
+                    try:
+                        os.killpg(process.pid, signal.SIGTERM)
+                    except ProcessLookupError:
+                        pass
+                    try:
+                        process.wait(timeout=3.0)
+                    except subprocess.TimeoutExpired:
+                        try:
+                            os.killpg(process.pid, signal.SIGKILL)
+                        except ProcessLookupError:
+                            pass
+                        process.wait(timeout=3.0)
+        finally:
+            log = self._logs.pop(process.pid, None)
+            if log is not None:
+                log.close()

devagent_physical_engine/ros2/doctor.py

@@ -0,0 +1,116 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+import platform
+
+from .commands import RosCommandRunner
+
+
+@dataclass(frozen=True, slots=True)
+class RosCheck:
+    name: str
+    passed: bool
+    detail: str
+    required: bool = True
+
+
+@dataclass(frozen=True, slots=True)
+class RosDoctorReport:
+    checks: tuple[RosCheck, ...]
+
+    @property
+    def passed(self) -> bool:
+        return all(check.passed for check in self.checks if check.required)
+
+    @property
+    def failures(self) -> tuple[RosCheck, ...]:
+        return tuple(check for check in self.checks if check.required and not check.passed)
+
+
+REQUIRED_PACKAGES = (
+    "ur_simulation_gz",
+    "ur_robot_driver",
+    "ur_moveit_config",
+    "controller_manager",
+)
+
+
+class RosDoctor:
+    def __init__(
+        self,
+        runner: RosCommandRunner,
+        *,
+        ros_setup: Path | str = "/opt/ros/jazzy/setup.bash",
+        os_release_path: Path | str = "/etc/os-release",
+    ) -> None:
+        self.runner = runner
+        self.ros_setup = Path(ros_setup)
+        self.os_release_path = Path(os_release_path)
+
+    def _os_release(self) -> dict[str, str]:
+        values: dict[str, str] = {}
+        try:
+            text = self.os_release_path.read_text(encoding="utf-8")
+        except OSError:
+            return values
+        for line in text.splitlines():
+            if "=" not in line or line.lstrip().startswith("#"):
+                continue
+            key, value = line.split("=", 1)
+            values[key.strip()] = value.strip().strip('"')
+        return values
+
+    def run(self) -> RosDoctorReport:
+        checks: list[RosCheck] = []
+        checks.append(
+            RosCheck(
+                "linux",
+                platform.system() == "Linux",
+                platform.system(),
+            )
+        )
+        os_release = self._os_release()
+        checks.append(
+            RosCheck(
+                "ubuntu_24_04",
+                os_release.get("ID") == "ubuntu" and os_release.get("VERSION_ID") == "24.04",
+                f"{os_release.get('ID', 'unknown')} {os_release.get('VERSION_ID', 'unknown')}",
+            )
+        )
+        checks.append(
+            RosCheck(
+                "ros_setup",
+                self.ros_setup.expanduser().is_file(),
+                str(self.ros_setup),
+            )
+        )
+        if not checks[-1].passed:
+            return RosDoctorReport(tuple(checks))
+
+        distro = self.runner.run(["bash", "-lc", "printf %s \"$ROS_DISTRO\""], timeout_s=5.0)
+        checks.append(
+            RosCheck(
+                "ros_distro_jazzy",
+                distro.ok and distro.stdout.strip() == "jazzy",
+                distro.stdout.strip() or distro.stderr.strip() or "unknown",
+            )
+        )
+
+        ros_cli = self.runner.run(["ros2", "--help"], timeout_s=5.0)
+        checks.append(RosCheck("ros2_cli", ros_cli.ok, "available" if ros_cli.ok else ros_cli.stderr.strip()))
+
+        gz = self.runner.run(["bash", "-lc", "command -v gz"], timeout_s=5.0)
+        checks.append(RosCheck("gazebo_gz", gz.ok and bool(gz.stdout.strip()), gz.stdout.strip() or "missing"))
+
+        for package in REQUIRED_PACKAGES:
+            result = self.runner.run(["ros2", "pkg", "prefix", package], timeout_s=5.0)
+            checks.append(
+                RosCheck(
+                    f"package:{package}",
+                    result.ok and bool(result.stdout.strip()),
+                    result.stdout.strip() if result.ok else (result.stderr.strip() or "missing"),
+                )
+            )
+
+        return RosDoctorReport(tuple(checks))

devagent_physical_engine/ros2/fk_probe.py

@@ -0,0 +1,83 @@
+from __future__ import annotations
+
+import argparse
+import json
+from math import dist
+from pathlib import Path
+from typing import Any
+
+
+def _wait(node, future, timeout_s: float) -> bool:
+    import rclpy
+    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_s)
+    return future.done()
+
+
+def compute(path: Path, timeout_s: float) -> dict[str, Any]:
+    payload = json.loads(path.read_text(encoding="utf-8"))
+    joint_names = list(payload["joint_names"])
+    samples = list(payload["samples"])
+    target = list(payload["target_positions_rad"])
+    planning_frame = str(payload["planning_frame"])
+    link_name = str(payload["link_name"])
+
+    import rclpy
+    from moveit_msgs.msg import MoveItErrorCodes
+    from moveit_msgs.srv import GetPositionFK
+
+    rclpy.init()
+    node = rclpy.create_node("devagent_fk_probe")
+    client = node.create_client(GetPositionFK, "/compute_fk")
+    try:
+        if not client.wait_for_service(timeout_sec=timeout_s):
+            return {"success": False, "code": "compute_fk_service_unavailable"}
+
+        def fk(positions: list[float]) -> tuple[float, float, float]:
+            request = GetPositionFK.Request()
+            request.header.frame_id = planning_frame
+            request.fk_link_names = [link_name]
+            request.robot_state.is_diff = True
+            request.robot_state.joint_state.name = joint_names
+            request.robot_state.joint_state.position = positions
+            future = client.call_async(request)
+            if not _wait(node, future, timeout_s):
+                raise RuntimeError("compute_fk_timeout")
+            response = future.result()
+            if response is None or response.error_code.val != MoveItErrorCodes.SUCCESS or not response.pose_stamped:
+                raise RuntimeError("compute_fk_failed")
+            pose = response.pose_stamped[0].pose.position
+            return float(pose.x), float(pose.y), float(pose.z)
+
+        positions = [fk([float(value) for value in sample["positions_rad"]]) for sample in samples]
+        target_position = fk([float(value) for value in target])
+        path_length = sum(dist(first, second) for first, second in zip(positions, positions[1:]))
+        final_error = dist(positions[-1], target_position) if positions else None
+        return {
+            "success": bool(positions),
+            "code": "moveit_fk_metrics_measured" if positions else "moveit_fk_samples_empty",
+            "path_length_m": path_length if positions else None,
+            "final_tcp_error_m": final_error,
+            "sample_count": len(positions),
+            "link_name": link_name,
+            "planning_frame": planning_frame,
+        }
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--input", required=True)
+    parser.add_argument("--timeout", type=float, default=10.0)
+    args = parser.parse_args()
+    try:
+        result = compute(Path(args.input).expanduser().resolve(), float(args.timeout))
+    except Exception as exc:
+        result = {"success": False, "code": f"fk_probe_error:{type(exc).__name__}"}
+    print(json.dumps(result, sort_keys=True))
+    return 0 if result.get("success") else 2
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())