dora-ulite6/dora_ulite6/config.py

"""Everything the node reads from the environment, parsed once at startup.

Parsing happens here rather than where each value is used, so an unusable
setting stops the node before it joins the dataflow instead of surfacing on the
first motion — the same reason the ZED node parses its depth mode up front.
"""

import os
from dataclasses import dataclass, field

from dora_ulite6.effectors import EndEffector, get_effector
from dora_ulite6.pose import RobotPose

INIT_MODES = ("none", "home", "pose")


def env_str(name: str, default: str) -> str:
    """Return the variable, or ``default`` when it is unset."""
    return os.getenv(name, default)


def env_float(name: str, default: float) -> float:
    """Return the variable as a float, naming it if it is not one."""
    value = os.getenv(name)
    if value is None or not value.strip():
        return default
    try:
        return float(value)
    except ValueError:
        raise ValueError(f"{name} must be a number, not {value!r}") from None


def env_int(name: str, default: int) -> int:
    """Return the variable as an integer, naming it if it is not one."""
    value = os.getenv(name)
    if value is None or not value.strip():
        return default
    try:
        return int(value)
    except ValueError:
        raise ValueError(f"{name} must be an integer, not {value!r}") from None


@dataclass
class MotionConfig:
    """Driver-level motion limits and timeouts.

    The arrival tolerances are wide on purpose: whether a motion happened is
    decided by the controller's error, and these only catch an arm that never
    left where it stood. Tightening them starts refusing moves that worked.
    """

    arrival_tolerance_mm: float = 25.0
    arrival_tolerance_deg: float = 15.0
    arrival_timeout_s: float = 2.0
    # How clear of its own bound a joint solution must stay for a pose to be
    # commanded, and how many points along the straight line are checked.
    joint_limit_margin_deg: float = 1.0
    path_samples: int = 4
    ready_timeout_s: float = 3.0
    home_timeout_s: float = 20.0
    tool_settle_s: float = 2.0
    # How long a jog stream may go quiet before this node stops the arm. The
    # velocity is set on the controller once and held there indefinitely — see
    # `ULite6.set_cartesian_velocity` — so nothing expires it but us: this is
    # the deadman behind a sender that dies mid-jog, enforced by `_expire_jog`.
    velocity_command_ttl_s: float = 0.5

    @classmethod
    def from_env(cls) -> "MotionConfig":
        """Read the motion limits and timeouts from the environment."""
        return cls(
            arrival_tolerance_mm=env_float("ARRIVAL_TOLERANCE_MM", 25.0),
            arrival_tolerance_deg=env_float("ARRIVAL_TOLERANCE_DEG", 15.0),
            arrival_timeout_s=env_float("ARRIVAL_TIMEOUT_S", 2.0),
            joint_limit_margin_deg=env_float("JOINT_LIMIT_MARGIN_DEG", 1.0),
            path_samples=env_int("PATH_SAMPLES", 4),
            ready_timeout_s=env_float("READY_TIMEOUT_S", 3.0),
            home_timeout_s=env_float("HOME_TIMEOUT_S", 20.0),
            tool_settle_s=env_float("TOOL_SETTLE_S", 2.0),
            velocity_command_ttl_s=env_float("VELOCITY_COMMAND_TTL_S", 0.5),
        )


@dataclass
class Config:
    """The node's settings: where the arm is, what it carries, and how it starts."""

    robot_ip: str = "192.168.1.192"
    effector: EndEffector = field(default_factory=lambda: get_effector("none"))
    # The mounted tool's own reach past the flange, plus TOOL_OFFSET_MM.
    tool_offset_mm: float = 0.0
    default_speed: float = 30.0
    default_joint_speed: float = 20.0
    # How often the control thread re-reads the arm. The dataflow tick only
    # decides how often the newest reading is published.
    telemetry_interval_s: float = 0.1
    # How long to wait before trying an unreachable arm again.
    reconnect_interval_s: float = 5.0
    init_mode: str = "none"
    init_pose: RobotPose = field(default_factory=lambda: RobotPose(300.0, 0.0, 250.0))
    init_speed: float = 50.0
    motion: MotionConfig = field(default_factory=MotionConfig)

    @classmethod
    def from_env(cls) -> "Config":
        """Read every setting from the environment, refusing one the node cannot use."""
        effector = get_effector(env_str("TOOL_TYPE", "none").lower())
        init_mode = env_str("INIT_MODE", "none").lower()
        if init_mode not in INIT_MODES:
            raise ValueError(f"INIT_MODE must be one of {', '.join(INIT_MODES)}, not {init_mode!r}")
        return cls(
            robot_ip=env_str("ROBOT_IP", "192.168.1.192"),
            effector=effector,
            tool_offset_mm=effector.offset_mm + env_float("TOOL_OFFSET_MM", 0.0),
            default_speed=env_float("DEFAULT_SPEED", 30.0),
            default_joint_speed=env_float("DEFAULT_JOINT_SPEED", 20.0),
            telemetry_interval_s=env_int("TELEMETRY_INTERVAL_MS", 100) / 1000.0,
            reconnect_interval_s=env_float("RECONNECT_INTERVAL_S", 5.0),
            init_mode=init_mode,
            init_pose=RobotPose(
                x=env_float("INIT_X", 300.0),
                y=env_float("INIT_Y", 0.0),
                z=env_float("INIT_Z", 250.0),
                roll=env_float("INIT_ROLL", 180.0),
                pitch=env_float("INIT_PITCH", 0.0),
                yaw=env_float("INIT_YAW", 0.0),
            ),
            init_speed=env_float("INIT_SPEED", 50.0),
            motion=MotionConfig.from_env(),
        )