griip-sdk-dev 0.4.0.dev24238__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- griip_sdk/__init__.py +77 -0
- griip_sdk/calibration/__init__.py +0 -0
- griip_sdk/calibration/app.py +638 -0
- griip_sdk/calibration/apriltag_pose.py +101 -0
- griip_sdk/calibration/camera_factory.py +105 -0
- griip_sdk/calibration/cartesian_pose.py +220 -0
- griip_sdk/calibration/config_store.py +131 -0
- griip_sdk/calibration/hand_eye_calib/README.md +109 -0
- griip_sdk/calibration/hand_eye_calib/__init__.py +40 -0
- griip_sdk/calibration/hand_eye_calib/config/routine.yaml +7 -0
- griip_sdk/calibration/hand_eye_calib/core.py +692 -0
- griip_sdk/calibration/hand_eye_calib/main.py +596 -0
- griip_sdk/calibration/hand_eye_calib/planning.py +122 -0
- griip_sdk/calibration/hand_eye_calib/routines.py +223 -0
- griip_sdk/calibration/hand_eye_calib/run_hand_eye_calibration.py +131 -0
- griip_sdk/calibration/hand_eye_calib/transforms.py +57 -0
- griip_sdk/calibration/mqtt_publisher.py +98 -0
- griip_sdk/calibration/mqtt_topics.py +15 -0
- griip_sdk/calibration/routines.py +36 -0
- griip_sdk/calibration/scanning/__init__.py +11 -0
- griip_sdk/calibration/scanning/environment_scanning.py +381 -0
- griip_sdk/calibration/scanning/run_environment_scanning.py +142 -0
- griip_sdk/calibration/tests/__init__.py +0 -0
- griip_sdk/calibration/tests/conftest.py +33 -0
- griip_sdk/calibration/tests/test_calibration_app.py +822 -0
- griip_sdk/calibration/tests/test_camera_factory.py +126 -0
- griip_sdk/calibration/tests/test_environment_scanning.py +547 -0
- griip_sdk/calibration/tests/test_hand_eye_calibration_service.py +524 -0
- griip_sdk/calibration/tests/test_hand_eye_helpers.py +211 -0
- griip_sdk/calibration/tests/test_transforms.py +126 -0
- griip_sdk/calibration/types.py +21 -0
- griip_sdk/calibration/units.py +104 -0
- griip_sdk/cell/__init__.py +22 -0
- griip_sdk/cell/builders.py +631 -0
- griip_sdk/cell/config.py +505 -0
- griip_sdk/cell/end_effector_collision_model.py +160 -0
- griip_sdk/cell/initialize.py +282 -0
- griip_sdk/cell/loader.py +102 -0
- griip_sdk/cell/mock_builders.py +114 -0
- griip_sdk/cell/parts.py +63 -0
- griip_sdk/cell/recipes.py +872 -0
- griip_sdk/cell/tests/__init__.py +0 -0
- griip_sdk/cell/tests/conftest.py +7 -0
- griip_sdk/cell/tests/test_builders.py +13 -0
- griip_sdk/cell/tests/test_end_effector_collision_model.py +118 -0
- griip_sdk/cell/tests/test_initialize.py +458 -0
- griip_sdk/cell/tests/test_loader.py +70 -0
- griip_sdk/cell/tests/test_mock_cell_smoke.py +54 -0
- griip_sdk/cell/tests/test_parts.py +55 -0
- griip_sdk/cell/tests/test_recipes.py +156 -0
- griip_sdk/cell/tests/test_recording_config.py +113 -0
- griip_sdk/errors.py +16 -0
- griip_sdk/grpc_adapters/__init__.py +0 -0
- griip_sdk/grpc_adapters/servicer_as_stub.py +166 -0
- griip_sdk/hardware/__init__.py +0 -0
- griip_sdk/hardware/bin_shaker.py +86 -0
- griip_sdk/hardware/camera.py +300 -0
- griip_sdk/hardware/fanuc_robot_client.py +266 -0
- griip_sdk/hardware/ft_sensor.py +352 -0
- griip_sdk/hardware/gripper.py +339 -0
- griip_sdk/hardware/grpc_vision_backend.py +123 -0
- griip_sdk/hardware/io_client.py +194 -0
- griip_sdk/hardware/mmai_vision_backend.py +265 -0
- griip_sdk/hardware/mocks/__init__.py +0 -0
- griip_sdk/hardware/mocks/camera_mock.py +62 -0
- griip_sdk/hardware/mocks/gripper_client_mock.py +135 -0
- griip_sdk/hardware/mocks/io_client_mock.py +28 -0
- griip_sdk/hardware/mocks/robot_client_mock.py +584 -0
- griip_sdk/hardware/onrobot_3fg15_gripper.py +103 -0
- griip_sdk/hardware/robot_client.py +1692 -0
- griip_sdk/hardware/robotiq_2f140_gripper.py +327 -0
- griip_sdk/hardware/sim_gripper.py +365 -0
- griip_sdk/hardware/sim_hand_eye.py +138 -0
- griip_sdk/hardware/sim_robot_client.py +189 -0
- griip_sdk/hardware/suction_gripper.py +112 -0
- griip_sdk/hardware/tests/__init__.py +0 -0
- griip_sdk/hardware/tests/conftest.py +8 -0
- griip_sdk/hardware/tests/test_call_with_timeout.py +56 -0
- griip_sdk/hardware/tests/test_command_state_fanout.py +131 -0
- griip_sdk/hardware/tests/test_gripper_command_listeners.py +90 -0
- griip_sdk/hardware/tests/test_grpc_vision_backend.py +220 -0
- griip_sdk/hardware/tests/test_mmai_vision_backend.py +312 -0
- griip_sdk/hardware/tests/test_reset_to_normal.py +101 -0
- griip_sdk/hardware/tests/test_robot_client_base.py +48 -0
- griip_sdk/hardware/tests/test_robot_client_lifecycle.py +323 -0
- griip_sdk/hardware/tests/test_robot_feedback_fanout.py +154 -0
- griip_sdk/hardware/tests/test_robotiq_2f140_gripper.py +86 -0
- griip_sdk/hardware/tests/test_robotiq_gripper_bus_lock.py +78 -0
- griip_sdk/hardware/tests/test_sim_gripper.py +230 -0
- griip_sdk/hardware/tests/test_sim_hand_eye.py +226 -0
- griip_sdk/hardware/tests/test_sim_robot_client.py +196 -0
- griip_sdk/hardware/tests/test_suction_gripper.py +82 -0
- griip_sdk/hardware/tests/test_tower_light.py +179 -0
- griip_sdk/hardware/tests/test_vision_codec.py +73 -0
- griip_sdk/hardware/tests/test_vrmcs_tcp_guard.py +81 -0
- griip_sdk/hardware/torquing_client.py +23 -0
- griip_sdk/hardware/tower_light.py +84 -0
- griip_sdk/hardware/ur_robot_client.py +88 -0
- griip_sdk/hardware/vision_codec.py +77 -0
- griip_sdk/log_utils.py +42 -0
- griip_sdk/modeling/__init__.py +0 -0
- griip_sdk/modeling/sphere_generation_tool/README.md +187 -0
- griip_sdk/modeling/sphere_generation_tool/__init__.py +8 -0
- griip_sdk/modeling/sphere_generation_tool/__main__.py +242 -0
- griip_sdk/modeling/sphere_generation_tool/_actors.py +278 -0
- griip_sdk/modeling/sphere_generation_tool/_geometry.py +55 -0
- griip_sdk/modeling/sphere_generation_tool/editor.py +1142 -0
- griip_sdk/modeling/sphere_generation_tool/object_io.py +53 -0
- griip_sdk/modeling/sphere_generation_tool/urdf_io.py +315 -0
- griip_sdk/modeling/tests/__init__.py +0 -0
- griip_sdk/modeling/tests/conftest.py +123 -0
- griip_sdk/modeling/tests/test_editor_math.py +80 -0
- griip_sdk/modeling/tests/test_editor_smoke.py +471 -0
- griip_sdk/modeling/tests/test_mesh_loading.py +66 -0
- griip_sdk/modeling/tests/test_object_io.py +72 -0
- griip_sdk/modeling/tests/test_urdf_io.py +367 -0
- griip_sdk/orchestration/__init__.py +129 -0
- griip_sdk/orchestration/bin_cuboid_extraction.py +77 -0
- griip_sdk/orchestration/bin_picking_manager.py +580 -0
- griip_sdk/orchestration/bin_state.py +85 -0
- griip_sdk/orchestration/callbacks.py +60 -0
- griip_sdk/orchestration/cycle_pause_gate.py +92 -0
- griip_sdk/orchestration/errors.py +27 -0
- griip_sdk/orchestration/execution/__init__.py +18 -0
- griip_sdk/orchestration/execution/grasp_executor.py +105 -0
- griip_sdk/orchestration/execution/parallel_jaw_executor.py +132 -0
- griip_sdk/orchestration/execution/suction_executor.py +53 -0
- griip_sdk/orchestration/execution/tests/__init__.py +0 -0
- griip_sdk/orchestration/execution/tests/test_parallel_jaw_executor.py +164 -0
- griip_sdk/orchestration/execution/three_finger_executor.py +144 -0
- griip_sdk/orchestration/grasp_provider.py +258 -0
- griip_sdk/orchestration/grasp_queue.py +107 -0
- griip_sdk/orchestration/part_setup.py +52 -0
- griip_sdk/orchestration/perception_inputs.py +187 -0
- griip_sdk/orchestration/pick_and_place_manager.py +1514 -0
- griip_sdk/orchestration/pick_generator.py +230 -0
- griip_sdk/orchestration/placement_planner.py +365 -0
- griip_sdk/orchestration/planning.py +59 -0
- griip_sdk/orchestration/proto_conversions.py +113 -0
- griip_sdk/orchestration/roi_scheduler.py +150 -0
- griip_sdk/orchestration/scoop_executor.py +122 -0
- griip_sdk/orchestration/tests/__init__.py +0 -0
- griip_sdk/orchestration/tests/conftest.py +156 -0
- griip_sdk/orchestration/tests/test_bin_picking_manager.py +485 -0
- griip_sdk/orchestration/tests/test_bin_state.py +160 -0
- griip_sdk/orchestration/tests/test_cycle_pause_gate.py +130 -0
- griip_sdk/orchestration/tests/test_grasp_provider.py +307 -0
- griip_sdk/orchestration/tests/test_part_setup.py +69 -0
- griip_sdk/orchestration/tests/test_perception_inputs.py +122 -0
- griip_sdk/orchestration/tests/test_phase_collision_spheres.py +57 -0
- griip_sdk/orchestration/tests/test_pick_and_place_manager.py +760 -0
- griip_sdk/orchestration/tests/test_pick_generator.py +130 -0
- griip_sdk/orchestration/tests/test_placement_planner.py +333 -0
- griip_sdk/orchestration/tests/test_planning.py +64 -0
- griip_sdk/orchestration/tests/test_proto_conversions.py +96 -0
- griip_sdk/orchestration/tests/test_scoop_executor.py +106 -0
- griip_sdk/orchestration/trajectory_store.py +170 -0
- griip_sdk/orchestration/types.py +466 -0
- griip_sdk/prototyping/__init__.py +0 -0
- griip_sdk/prototyping/config/__init__.py +20 -0
- griip_sdk/prototyping/config/registry.py +22 -0
- griip_sdk/prototyping/in_process_api.py +59 -0
- griip_sdk/prototyping/read_tcp.py +67 -0
- griip_sdk/prototyping/run_environment_scanning.py +99 -0
- griip_sdk/prototyping/run_hand_eye_calibration.py +104 -0
- griip_sdk/prototyping/run_linear_shuttle.py +556 -0
- griip_sdk/recording/__init__.py +71 -0
- griip_sdk/recording/foxglove_events.py +553 -0
- griip_sdk/recording/foxglove_joint_states.py +115 -0
- griip_sdk/recording/gripper_state_logger.py +203 -0
- griip_sdk/recording/gripper_state_mcap_recording.py +69 -0
- griip_sdk/recording/h264_video_logger.py +128 -0
- griip_sdk/recording/h264_video_mcap_recording.py +217 -0
- griip_sdk/recording/kpi_logger.py +186 -0
- griip_sdk/recording/kpi_mcap_recording.py +143 -0
- griip_sdk/recording/kpi_timer.py +148 -0
- griip_sdk/recording/mcap_recording.py +209 -0
- griip_sdk/recording/memory_utils.py +19 -0
- griip_sdk/recording/robot_state_logger.py +133 -0
- griip_sdk/recording/rotating_mcap_writer.py +326 -0
- griip_sdk/recording/tests/test_foxglove_event_publisher.py +324 -0
- griip_sdk/recording/tests/test_foxglove_joint_states.py +26 -0
- griip_sdk/recording/tests/test_kpi_timer.py +346 -0
- griip_sdk/recording/tests/test_mcap_foxglove_event_publisher.py +355 -0
- griip_sdk/recording/tests/test_mcap_gripper_state_logger.py +334 -0
- griip_sdk/recording/tests/test_mcap_h264_video_logger.py +118 -0
- griip_sdk/recording/tests/test_mcap_kpi_logger.py +72 -0
- griip_sdk/recording/tests/test_mcap_robot_state_logger.py +253 -0
- griip_sdk/recording/tests/test_session.py +87 -0
- griip_sdk/tests/__init__.py +0 -0
- griip_sdk/tests/test_log_utils.py +81 -0
- griip_sdk/tests/test_servicer_as_stub.py +69 -0
- griip_sdk_dev-0.4.0.dev24238.dist-info/METADATA +513 -0
- griip_sdk_dev-0.4.0.dev24238.dist-info/RECORD +195 -0
- griip_sdk_dev-0.4.0.dev24238.dist-info/WHEEL +4 -0
|
@@ -0,0 +1,638 @@
|
|
|
1
|
+
"""High-level recipe for assembling a calibration app from a cell YAML.
|
|
2
|
+
|
|
3
|
+
Parallel to :mod:`griip_sdk.cell.recipes`: ``build_calibration_app`` wires a
|
|
4
|
+
calibration workflow the way ``build_pick_only_manager`` wires picking. The
|
|
5
|
+
returned :class:`CalibrationApp` drives hand-eye calibration and the
|
|
6
|
+
operator-driven environment scan. See the README for the consumer walkthrough.
|
|
7
|
+
|
|
8
|
+
Protos: requests use ``griip_core.generated.griip_pb2``; the mmai-griip-api
|
|
9
|
+
server serves the schema-identical ``griip.proto``. The two are kept in
|
|
10
|
+
lockstep -- if they diverge, gRPC calls silently misbehave.
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import logging
|
|
16
|
+
from contextlib import contextmanager
|
|
17
|
+
from datetime import datetime
|
|
18
|
+
from pathlib import Path
|
|
19
|
+
from typing import Any, Callable, Iterator, cast
|
|
20
|
+
|
|
21
|
+
import grpc
|
|
22
|
+
import numpy as np
|
|
23
|
+
import numpy.typing as npt
|
|
24
|
+
from spatialmath import SE3, SO3
|
|
25
|
+
from vention.vision.v1.vision_service_abc import ImageServiceBackend
|
|
26
|
+
|
|
27
|
+
from griip_sdk.calibration.camera_factory import build_calibration_camera
|
|
28
|
+
from griip_sdk.calibration.config_store import (
|
|
29
|
+
DEFAULT_CALIBRATION_DATA_PATH,
|
|
30
|
+
CalibrationConfigStore,
|
|
31
|
+
utc_ts,
|
|
32
|
+
)
|
|
33
|
+
from griip_sdk.calibration.hand_eye_calib import (
|
|
34
|
+
CalibrationCancelledError,
|
|
35
|
+
HandEyeCalibrationService,
|
|
36
|
+
)
|
|
37
|
+
from griip_sdk.calibration.hand_eye_calib.planning import (
|
|
38
|
+
build_and_configure_hand_eye_planning_service,
|
|
39
|
+
resolve_hand_eye_planner_params,
|
|
40
|
+
)
|
|
41
|
+
from griip_sdk.calibration.hand_eye_calib.transforms import (
|
|
42
|
+
compute_camera_to_tcp_transform,
|
|
43
|
+
load_camera_to_flange_transform,
|
|
44
|
+
)
|
|
45
|
+
from griip_sdk.calibration.mqtt_publisher import MqttProgressPublisher, compose_on_progress
|
|
46
|
+
from griip_sdk.calibration.mqtt_topics import ENVIRONMENT_PROGRESS_TOPIC, HAND_EYE_PROGRESS_TOPIC
|
|
47
|
+
from griip_sdk.calibration.routines import DEFAULT_ENCIRCLE_ROUTINE
|
|
48
|
+
from griip_sdk.calibration.scanning import EnvironmentScanningService
|
|
49
|
+
from griip_sdk.calibration.types import PerceptionStub, PlanningStub
|
|
50
|
+
from griip_sdk.cell import load_cell_config
|
|
51
|
+
from griip_sdk.cell.builders import resolve_camera_calibration
|
|
52
|
+
from griip_sdk.cell.config import PrototypeCellConfig
|
|
53
|
+
from griip_sdk.cell.initialize import build_robot_initialize_request, send_initialize
|
|
54
|
+
from griip_sdk.errors import ConfigError
|
|
55
|
+
from griip_core.generated import griip_pb2 as pb
|
|
56
|
+
from griip_core.generated import griip_pb2_grpc
|
|
57
|
+
from griip_sdk.grpc_adapters.servicer_as_stub import ServicerAsStub
|
|
58
|
+
from griip_sdk.hardware.robot_client import VrmcsRobotClient, build_vrmcs_client
|
|
59
|
+
|
|
60
|
+
logger = logging.getLogger(__name__)
|
|
61
|
+
|
|
62
|
+
|
|
63
|
+
class CalibrationApp:
|
|
64
|
+
"""Returned by :func:`build_calibration_app`; exposes the calibration verbs.
|
|
65
|
+
|
|
66
|
+
Owns shared infrastructure (robot client, started camera client, planning +
|
|
67
|
+
perception stubs, calibration store) so repeated verb calls don't re-pay
|
|
68
|
+
setup. Per-verb services are built on demand.
|
|
69
|
+
"""
|
|
70
|
+
|
|
71
|
+
def __init__(
|
|
72
|
+
self,
|
|
73
|
+
*,
|
|
74
|
+
cell: PrototypeCellConfig,
|
|
75
|
+
config_path: Path,
|
|
76
|
+
robot_client: VrmcsRobotClient,
|
|
77
|
+
vision_backend: ImageServiceBackend,
|
|
78
|
+
cam_name: str,
|
|
79
|
+
camera_mxid: str,
|
|
80
|
+
sensor: str,
|
|
81
|
+
store: CalibrationConfigStore,
|
|
82
|
+
calib_data_root: Path | str,
|
|
83
|
+
planning_stub: PlanningStub,
|
|
84
|
+
perception_stub: PerceptionStub,
|
|
85
|
+
secondary_source_id: str = "right",
|
|
86
|
+
mqtt_publisher: MqttProgressPublisher | None = None,
|
|
87
|
+
strict_intrinsics: bool = True,
|
|
88
|
+
) -> None:
|
|
89
|
+
self._cell = cell
|
|
90
|
+
self._config_path = config_path
|
|
91
|
+
self._robot_client = robot_client
|
|
92
|
+
self._vision_backend = vision_backend
|
|
93
|
+
self._cam_name = cam_name
|
|
94
|
+
self._camera_mxid = camera_mxid
|
|
95
|
+
self._sensor = sensor
|
|
96
|
+
self._secondary_source_id = secondary_source_id
|
|
97
|
+
self._store = store
|
|
98
|
+
self._calib_data_root = calib_data_root
|
|
99
|
+
self._planning_stub = planning_stub
|
|
100
|
+
self._perception_stub = perception_stub
|
|
101
|
+
self._mqtt_publisher = mqtt_publisher
|
|
102
|
+
self._strict_intrinsics = strict_intrinsics
|
|
103
|
+
|
|
104
|
+
self._env_session_service: EnvironmentScanningService | None = None
|
|
105
|
+
self._env_session_sequence_dir: Path | None = None
|
|
106
|
+
self._env_session_next_index: int = 0
|
|
107
|
+
self._env_session_previous_tcp_offset: SE3 | None = None
|
|
108
|
+
self._reset_env_session_state()
|
|
109
|
+
|
|
110
|
+
# ----- Hand-eye calibration --------------------------------------
|
|
111
|
+
|
|
112
|
+
def run_hand_eye_calibration(
|
|
113
|
+
self,
|
|
114
|
+
*,
|
|
115
|
+
on_progress: Callable[[int, str, str], None] | None = None,
|
|
116
|
+
is_cancelled: Callable[[], bool] | None = None,
|
|
117
|
+
save_dir: Path | None = None,
|
|
118
|
+
routine: dict[str, Any] | None = None,
|
|
119
|
+
bootstrap_path: Path | None = None,
|
|
120
|
+
) -> SE3:
|
|
121
|
+
"""Plan to ``cell.positions.calibration_joints``, run the sphere routine, then return there.
|
|
122
|
+
|
|
123
|
+
Raises :class:`ConfigError` if ``calibration_joints`` is unset. Persists
|
|
124
|
+
the result under ``X_cam_flg__{cam_name}__{mxid}__{sensor}``. Emits exactly
|
|
125
|
+
one terminal ``on_progress`` tick (``success`` 100 / ``error`` 0 /
|
|
126
|
+
``cancelled`` 0), even on early failures, so consumers needn't wrap the call.
|
|
127
|
+
|
|
128
|
+
Args:
|
|
129
|
+
on_progress: ``(percent, status, message)`` callback.
|
|
130
|
+
is_cancelled: predicate polled between waypoints to stop cooperatively.
|
|
131
|
+
save_dir: output dir for artifacts; defaults to a timestamped ``/tmp`` dir.
|
|
132
|
+
routine: sphere-routine dict overriding
|
|
133
|
+
:data:`~griip_sdk.calibration.routines.DEFAULT_ENCIRCLE_ROUTINE`.
|
|
134
|
+
bootstrap_path: pre-computed bootstrap result to skip the bootstrap leg.
|
|
135
|
+
"""
|
|
136
|
+
# Compose the consumer's callback with the SDK's own MQTT publisher (if a
|
|
137
|
+
# broker was configured) so the terminal ticks below also reach the UI.
|
|
138
|
+
mqtt_progress = self._mqtt_publisher.make_on_progress(HAND_EYE_PROGRESS_TOPIC) if self._mqtt_publisher else None
|
|
139
|
+
effective_on_progress = compose_on_progress(on_progress, mqtt_progress)
|
|
140
|
+
try:
|
|
141
|
+
service = self._build_hand_eye_service(
|
|
142
|
+
save_dir=save_dir,
|
|
143
|
+
routine=routine,
|
|
144
|
+
bootstrap_path=bootstrap_path,
|
|
145
|
+
)
|
|
146
|
+
x_cam_flg: SE3 = service.calibrate(on_progress=effective_on_progress, is_cancelled=is_cancelled)
|
|
147
|
+
config_name = self._hand_eye_name()
|
|
148
|
+
self._store.add_new_version(config_name, {"X_cam_flg": x_cam_flg.A.tolist()})
|
|
149
|
+
logger.info("Saved %s under %s", config_name, self._store.root)
|
|
150
|
+
except CalibrationCancelledError:
|
|
151
|
+
self._emit_terminal(effective_on_progress, 0, "cancelled", "Hand-eye calibration cancelled")
|
|
152
|
+
raise
|
|
153
|
+
except Exception as exc:
|
|
154
|
+
self._emit_terminal(effective_on_progress, 0, "error", f"Hand-eye calibration failed: {exc}")
|
|
155
|
+
raise
|
|
156
|
+
else:
|
|
157
|
+
self._emit_terminal(effective_on_progress, 100, "success", "Hand-eye calibration complete")
|
|
158
|
+
return x_cam_flg
|
|
159
|
+
|
|
160
|
+
@staticmethod
|
|
161
|
+
def _emit_terminal(
|
|
162
|
+
on_progress: Callable[[int, str, str], None] | None,
|
|
163
|
+
percent: int,
|
|
164
|
+
status: str,
|
|
165
|
+
message: str,
|
|
166
|
+
) -> None:
|
|
167
|
+
"""Emit one guarded terminal ``on_progress`` tick (no-op if unset)."""
|
|
168
|
+
if on_progress is not None:
|
|
169
|
+
on_progress(percent, status, message)
|
|
170
|
+
|
|
171
|
+
def last_hand_eye_calibration_timestamp(self) -> datetime | None:
|
|
172
|
+
return self._store.timestamp_of(self._hand_eye_name())
|
|
173
|
+
|
|
174
|
+
# ----- Environment-scan session ----------------------------------
|
|
175
|
+
|
|
176
|
+
def start_environment_session(self, save_dir: Path | None = None) -> Path:
|
|
177
|
+
"""Open a new operator-driven environment-scan session.
|
|
178
|
+
|
|
179
|
+
Builds the :class:`EnvironmentScanningService`, creates the sequence
|
|
180
|
+
directory, persists post-crop intrinsics, and switches the robot into
|
|
181
|
+
freedrive. Raises ``RuntimeError`` if a session is already open; returns
|
|
182
|
+
the sequence directory.
|
|
183
|
+
"""
|
|
184
|
+
if self._env_session_service is not None:
|
|
185
|
+
raise RuntimeError(
|
|
186
|
+
"An environment-scan session is already open; call end_environment_session() first.",
|
|
187
|
+
)
|
|
188
|
+
|
|
189
|
+
x_tcp_flg = self._env_scan_tcp_offset_transform()
|
|
190
|
+
previous_tcp_offset = self._robot_client.X_tcp_flg
|
|
191
|
+
self._robot_client.set_tcp_offset(x_tcp_flg)
|
|
192
|
+
|
|
193
|
+
try:
|
|
194
|
+
service = self._build_env_scan_service(save_dir=save_dir, x_tcp_flg=x_tcp_flg)
|
|
195
|
+
sequence_dir = service.start_session()
|
|
196
|
+
next_index = service.resume_index(sequence_dir)
|
|
197
|
+
except Exception:
|
|
198
|
+
# Restore the frame mutated above so a failed start can't strand the
|
|
199
|
+
# robot on the env-scan offset -- and so a retry doesn't capture the
|
|
200
|
+
# mutated offset as its "previous" and later restore to the wrong frame.
|
|
201
|
+
self._robot_client.set_tcp_offset(previous_tcp_offset)
|
|
202
|
+
raise
|
|
203
|
+
|
|
204
|
+
# Commit session state only once the start fully succeeded.
|
|
205
|
+
self._env_session_previous_tcp_offset = previous_tcp_offset
|
|
206
|
+
self._env_session_service = service
|
|
207
|
+
self._env_session_sequence_dir = sequence_dir
|
|
208
|
+
self._env_session_next_index = next_index
|
|
209
|
+
self._publish_env_progress(
|
|
210
|
+
"in_progress",
|
|
211
|
+
f"Session started: {sequence_dir.name}",
|
|
212
|
+
extra={"sessionName": sequence_dir.name, "photoCount": self._env_session_next_index},
|
|
213
|
+
)
|
|
214
|
+
return sequence_dir
|
|
215
|
+
|
|
216
|
+
def take_environment_image(self) -> int:
|
|
217
|
+
"""Capture one frame in the current session and return its index.
|
|
218
|
+
|
|
219
|
+
Reads the TCP pose, grabs stereo + depth, and writes the per-frame files
|
|
220
|
+
under the active sequence directory (robot stays in freedrive between
|
|
221
|
+
captures). Raises ``RuntimeError`` if no session is open.
|
|
222
|
+
"""
|
|
223
|
+
service, sequence_dir = self._require_open_session()
|
|
224
|
+
frame_index = self._env_session_next_index
|
|
225
|
+
service.capture_frame(sequence_dir, frame_index)
|
|
226
|
+
self._env_session_next_index += 1
|
|
227
|
+
self._publish_env_progress(
|
|
228
|
+
"in_progress",
|
|
229
|
+
f"Captured photo {self._env_session_next_index}",
|
|
230
|
+
extra={
|
|
231
|
+
"sessionName": sequence_dir.name,
|
|
232
|
+
"photoCount": self._env_session_next_index,
|
|
233
|
+
},
|
|
234
|
+
)
|
|
235
|
+
return frame_index
|
|
236
|
+
|
|
237
|
+
def end_environment_session(self) -> Path:
|
|
238
|
+
"""Close the active session, restore the robot, and persist a store entry
|
|
239
|
+
pointing at the sequence directory.
|
|
240
|
+
|
|
241
|
+
Returns the sequence directory. Raises ``RuntimeError`` if no session is open.
|
|
242
|
+
"""
|
|
243
|
+
service, sequence_dir = self._require_open_session()
|
|
244
|
+
photo_count = self._env_session_next_index
|
|
245
|
+
try:
|
|
246
|
+
service.end_session()
|
|
247
|
+
finally:
|
|
248
|
+
# Restore the TCP offset mutated at session start (so a later
|
|
249
|
+
# hand-eye run plans from the same frame), then clear session state.
|
|
250
|
+
# Both must run even if end_session() raises, or a failed close
|
|
251
|
+
# leaves a zombie service the next end_environment_session() would
|
|
252
|
+
# re-close. Restore before reset: the reset clears the saved offset.
|
|
253
|
+
if self._env_session_previous_tcp_offset is not None:
|
|
254
|
+
self._robot_client.set_tcp_offset(self._env_session_previous_tcp_offset)
|
|
255
|
+
self._reset_env_session_state()
|
|
256
|
+
config_name = self._env_scan_name()
|
|
257
|
+
self._store.add_new_version(config_name, {"sequence_dir": str(sequence_dir)})
|
|
258
|
+
logger.info("Saved %s under %s", config_name, self._store.root)
|
|
259
|
+
self._publish_env_progress(
|
|
260
|
+
"success",
|
|
261
|
+
f"Session ended: {photo_count} photo(s) saved",
|
|
262
|
+
extra={"sessionName": None, "photoCount": photo_count},
|
|
263
|
+
)
|
|
264
|
+
return sequence_dir
|
|
265
|
+
|
|
266
|
+
def _publish_env_progress(self, status: str, message: str, *, extra: dict[str, Any]) -> None:
|
|
267
|
+
"""Publish an environment-scan progress event to MQTT (no-op if unconfigured)."""
|
|
268
|
+
if self._mqtt_publisher is None:
|
|
269
|
+
return
|
|
270
|
+
payload = {"status": status, "message": message, **extra}
|
|
271
|
+
self._mqtt_publisher.publish(ENVIRONMENT_PROGRESS_TOPIC, payload)
|
|
272
|
+
|
|
273
|
+
def last_environment_scan_timestamp(self) -> datetime | None:
|
|
274
|
+
return self._store.timestamp_of(self._env_scan_name())
|
|
275
|
+
|
|
276
|
+
def _reset_env_session_state(self) -> None:
|
|
277
|
+
"""Clear the operator-driven environment-scan session state."""
|
|
278
|
+
self._env_session_service = None
|
|
279
|
+
self._env_session_sequence_dir = None
|
|
280
|
+
self._env_session_next_index = 0
|
|
281
|
+
self._env_session_previous_tcp_offset = None
|
|
282
|
+
|
|
283
|
+
def _require_open_session(self) -> tuple[EnvironmentScanningService, Path]:
|
|
284
|
+
"""Return the open session's ``(service, sequence_dir)`` or raise ``RuntimeError``."""
|
|
285
|
+
if self._env_session_service is None or self._env_session_sequence_dir is None:
|
|
286
|
+
raise RuntimeError(
|
|
287
|
+
"No environment-scan session open; call start_environment_session() first.",
|
|
288
|
+
)
|
|
289
|
+
return self._env_session_service, self._env_session_sequence_dir
|
|
290
|
+
|
|
291
|
+
# ----- Name builders ----------------------------------------------
|
|
292
|
+
|
|
293
|
+
def _hand_eye_name(self) -> str:
|
|
294
|
+
return CalibrationConfigStore.format_config_name(
|
|
295
|
+
"X_cam_flg",
|
|
296
|
+
self._cam_name,
|
|
297
|
+
self._camera_mxid,
|
|
298
|
+
self._sensor,
|
|
299
|
+
)
|
|
300
|
+
|
|
301
|
+
def _env_scan_name(self) -> str:
|
|
302
|
+
return CalibrationConfigStore.format_config_name("env_scan", self._cam_name, self._camera_mxid)
|
|
303
|
+
|
|
304
|
+
# ----- Service builders -------------------------------------------
|
|
305
|
+
|
|
306
|
+
def _resolve_calibration(self) -> tuple[npt.NDArray[np.float64], npt.NDArray[np.float64], int, int, float]:
|
|
307
|
+
"""Resolve intrinsics/distortion/baseline for ``self._sensor`` via the vision ABC.
|
|
308
|
+
|
|
309
|
+
If the store has no calibio entry: ``strict_intrinsics=True`` (default)
|
|
310
|
+
raises :class:`FileNotFoundError` naming the missing config;
|
|
311
|
+
``strict_intrinsics=False`` logs a warning and proceeds with whatever
|
|
312
|
+
calibration ``vision_backend`` currently reports -- the camera-swap
|
|
313
|
+
escape hatch for a fresh camera with no calibio data yet. Either way,
|
|
314
|
+
the backend itself owns applying calibio (see
|
|
315
|
+
:class:`~griip_sdk.hardware.mmai_vision_backend.MmaiVisionServiceBackend`);
|
|
316
|
+
this only gates the strict check and reads the result.
|
|
317
|
+
|
|
318
|
+
The calibio store gate only applies to the legacy ``mmai`` camera
|
|
319
|
+
backend; native gRPC vision services own their calibration, so their
|
|
320
|
+
reported intrinsics are authoritative as-is.
|
|
321
|
+
|
|
322
|
+
Returns ``(cam_intr, dist_coeffs, im_width, im_height, stereo_baseline)``.
|
|
323
|
+
"""
|
|
324
|
+
if self._cell.camera.backend == "mmai":
|
|
325
|
+
intr_config_name = CalibrationConfigStore.format_config_name(
|
|
326
|
+
"calibio",
|
|
327
|
+
self._cam_name,
|
|
328
|
+
self._camera_mxid,
|
|
329
|
+
)
|
|
330
|
+
if not self._store.list_versions(intr_config_name):
|
|
331
|
+
if self._strict_intrinsics:
|
|
332
|
+
raise FileNotFoundError(
|
|
333
|
+
f"No intrinsic calibration found in store for '{intr_config_name}'. "
|
|
334
|
+
f"Upload calibio intrinsics under {self._store.root / intr_config_name}, or "
|
|
335
|
+
"construct the app with `strict_intrinsics=False` to fall back to live "
|
|
336
|
+
"camera defaults.",
|
|
337
|
+
)
|
|
338
|
+
logger.warning(
|
|
339
|
+
"No calibio intrinsics for '%s' in store; falling back to live camera defaults (strict_intrinsics=False).",
|
|
340
|
+
intr_config_name,
|
|
341
|
+
)
|
|
342
|
+
cam_intr, dist_coeffs, im_width, im_height, stereo_baseline, _, _ = resolve_camera_calibration(
|
|
343
|
+
self._vision_backend, self._sensor, self._secondary_source_id
|
|
344
|
+
)
|
|
345
|
+
return cam_intr, dist_coeffs, im_width, im_height, stereo_baseline
|
|
346
|
+
|
|
347
|
+
def _resolve_calibration_joints(self) -> list[float]:
|
|
348
|
+
"""Return ``cell.positions.calibration_joints`` or raise :class:`ConfigError`.
|
|
349
|
+
|
|
350
|
+
The cell YAML is the single source of truth for the starting pose -- no
|
|
351
|
+
runtime fallback. A missing field is an operator-actionable config problem.
|
|
352
|
+
"""
|
|
353
|
+
joints = self._cell.positions.calibration_joints
|
|
354
|
+
if joints is None:
|
|
355
|
+
raise ConfigError(
|
|
356
|
+
f"cell.positions.calibration_joints is not set in {self._config_path}. "
|
|
357
|
+
"Commissioning needs to author the starting pose in the cell YAML before "
|
|
358
|
+
"calibration can run.",
|
|
359
|
+
)
|
|
360
|
+
return list(joints)
|
|
361
|
+
|
|
362
|
+
def _resolve_save_dir(self, save_dir: Path | None, default: str) -> Path:
|
|
363
|
+
"""Return ``save_dir`` (or a timestamped ``default``), created on disk."""
|
|
364
|
+
resolved = Path(save_dir) if save_dir is not None else Path(default)
|
|
365
|
+
resolved.mkdir(parents=True, exist_ok=True)
|
|
366
|
+
return resolved
|
|
367
|
+
|
|
368
|
+
def _build_hand_eye_service(
|
|
369
|
+
self,
|
|
370
|
+
*,
|
|
371
|
+
save_dir: Path | None,
|
|
372
|
+
routine: dict[str, Any] | None,
|
|
373
|
+
bootstrap_path: Path | None,
|
|
374
|
+
) -> HandEyeCalibrationService:
|
|
375
|
+
cam_intr, dist_coeffs, im_width, im_height, _stereo_baseline = self._resolve_calibration()
|
|
376
|
+
|
|
377
|
+
# Bundled routine wins by default; consumers override with `routine=`.
|
|
378
|
+
# The legacy hand_eye_routine.yaml-next-to-cell-yaml lookup is gone.
|
|
379
|
+
calibration_routine = routine if routine is not None else DEFAULT_ENCIRCLE_ROUTINE
|
|
380
|
+
|
|
381
|
+
resolved_save_dir = self._resolve_save_dir(save_dir, f"/tmp/hand_eye_calibration_{utc_ts()}")
|
|
382
|
+
|
|
383
|
+
return HandEyeCalibrationService(
|
|
384
|
+
robo=self._robot_client,
|
|
385
|
+
vision_backend=self._vision_backend,
|
|
386
|
+
store=self._store,
|
|
387
|
+
planning_stub=cast(griip_pb2_grpc.PlanningServiceStub, self._planning_stub),
|
|
388
|
+
config=calibration_routine,
|
|
389
|
+
cam_intr=cam_intr,
|
|
390
|
+
dist_coeffs=dist_coeffs,
|
|
391
|
+
im_width=im_width,
|
|
392
|
+
im_height=im_height,
|
|
393
|
+
sensor_name=self._sensor,
|
|
394
|
+
save_dir=str(resolved_save_dir),
|
|
395
|
+
calibration_joint_angles=self._resolve_calibration_joints(),
|
|
396
|
+
bootstrap_path=str(bootstrap_path) if bootstrap_path is not None else None,
|
|
397
|
+
velocity_scaling=self._cell.motion.calibration.velocity_scaling,
|
|
398
|
+
acceleration_scaling=self._cell.motion.calibration.acceleration_scaling,
|
|
399
|
+
)
|
|
400
|
+
|
|
401
|
+
def _env_scan_tcp_offset_transform(self) -> SE3:
|
|
402
|
+
"""Build the flange-to-TCP offset transform from ``cell.tool``."""
|
|
403
|
+
if self._cell.tool is None:
|
|
404
|
+
logger.warning(
|
|
405
|
+
"cell.tool is unset; defaulting tcp_offset to [0,0,0]. Env-scan output will be miscalibrated "
|
|
406
|
+
"for cells with a physically-mounted tool.",
|
|
407
|
+
)
|
|
408
|
+
tcp_offset = [0.0, 0.0, 0.0]
|
|
409
|
+
else:
|
|
410
|
+
tcp_offset = list(self._cell.tool.tcp_offset)
|
|
411
|
+
return SE3.Rt(R=SO3(), t=np.array(tcp_offset, dtype=np.float64))
|
|
412
|
+
|
|
413
|
+
def _build_env_scan_service(self, *, save_dir: Path | None, x_tcp_flg: SE3) -> EnvironmentScanningService:
|
|
414
|
+
cam_intr, _dist_coeffs, image_width, image_height, stereo_baseline = self._resolve_calibration()
|
|
415
|
+
|
|
416
|
+
x_cam_flg = load_camera_to_flange_transform(
|
|
417
|
+
camera_vendor=self._cam_name,
|
|
418
|
+
camera_mx_id=self._camera_mxid,
|
|
419
|
+
sensor=self._sensor,
|
|
420
|
+
calibration_data_path=self._calib_data_root,
|
|
421
|
+
)
|
|
422
|
+
x_cam_tcp = compute_camera_to_tcp_transform(x_cam_flg, x_tcp_flg)
|
|
423
|
+
|
|
424
|
+
resolved_save_dir = self._resolve_save_dir(save_dir, f"/data/vention/scene_scanning_dataset_{utc_ts()}")
|
|
425
|
+
|
|
426
|
+
return EnvironmentScanningService(
|
|
427
|
+
robot_client=self._robot_client,
|
|
428
|
+
vision_backend=self._vision_backend,
|
|
429
|
+
left_source_id=self._sensor,
|
|
430
|
+
right_source_id=self._secondary_source_id,
|
|
431
|
+
perception_stub=self._perception_stub,
|
|
432
|
+
camera_intrinsics=cam_intr,
|
|
433
|
+
x_cam_tcp=x_cam_tcp,
|
|
434
|
+
stereo_baseline=stereo_baseline,
|
|
435
|
+
save_dir=resolved_save_dir,
|
|
436
|
+
image_width=image_width,
|
|
437
|
+
image_height=image_height,
|
|
438
|
+
)
|
|
439
|
+
|
|
440
|
+
|
|
441
|
+
def _build_in_process_planning_stub(
|
|
442
|
+
robot_client: VrmcsRobotClient,
|
|
443
|
+
cell: PrototypeCellConfig,
|
|
444
|
+
cell_config_directory: Path,
|
|
445
|
+
*,
|
|
446
|
+
cuboids_path_override: str | None = None,
|
|
447
|
+
) -> PlanningStub:
|
|
448
|
+
# Lazy import: pulls mmai_griip_api transitively (dev-only dep; see
|
|
449
|
+
# pyproject.toml). Importing it at module top-level would break wheel installs.
|
|
450
|
+
from mmai_griip_api.api.planning_api import PlanningApi
|
|
451
|
+
from mmai_griip_api.api.service_registry import ServiceRegistry
|
|
452
|
+
|
|
453
|
+
planning_service = build_and_configure_hand_eye_planning_service(
|
|
454
|
+
robot_client,
|
|
455
|
+
cell,
|
|
456
|
+
cell_config_directory,
|
|
457
|
+
cuboids_path_override=cuboids_path_override,
|
|
458
|
+
)
|
|
459
|
+
registry = ServiceRegistry()
|
|
460
|
+
registry.planning_service = planning_service
|
|
461
|
+
return ServicerAsStub(PlanningApi(registry))
|
|
462
|
+
|
|
463
|
+
|
|
464
|
+
def _build_in_process_perception_stub(cell: PrototypeCellConfig) -> PerceptionStub:
|
|
465
|
+
# Lazy import: same reasoning as _build_in_process_planning_stub.
|
|
466
|
+
from mmai_griip_api.api.perception_api import PerceptionApi
|
|
467
|
+
from mmai_griip_api.api.service_registry import ServiceRegistry
|
|
468
|
+
from mmai_griip_api.services.perception.foundation_stereo_client import FoundationStereoClient
|
|
469
|
+
from mmai_griip_api.services.perception.perception_service import PerceptionService
|
|
470
|
+
|
|
471
|
+
foundation_stereo_client = FoundationStereoClient(host=cell.perception.services.foundation_stereo)
|
|
472
|
+
perception_service = PerceptionService(
|
|
473
|
+
sam_client=None,
|
|
474
|
+
foundation_pose_client=None,
|
|
475
|
+
foundation_stereo_client=foundation_stereo_client,
|
|
476
|
+
)
|
|
477
|
+
registry = ServiceRegistry()
|
|
478
|
+
registry.perception_service = perception_service
|
|
479
|
+
return ServicerAsStub(PerceptionApi(registry))
|
|
480
|
+
|
|
481
|
+
|
|
482
|
+
def _build_grpc_calibration_stubs(griip_api_url: str) -> tuple[grpc.Channel, PlanningStub, PerceptionStub]:
|
|
483
|
+
"""Open a gRPC channel to ``griip_api_url``; return ``(channel, planning_stub, perception_stub)``.
|
|
484
|
+
|
|
485
|
+
Uses the SDK's own generated stubs so callers don't need the dev-only
|
|
486
|
+
``mmai_griip_api`` package. The caller closes the channel.
|
|
487
|
+
"""
|
|
488
|
+
channel = grpc.insecure_channel(griip_api_url)
|
|
489
|
+
planning_stub = griip_pb2_grpc.PlanningServiceStub(channel)
|
|
490
|
+
perception_stub = griip_pb2_grpc.PerceptionServiceStub(channel)
|
|
491
|
+
return channel, planning_stub, perception_stub
|
|
492
|
+
|
|
493
|
+
|
|
494
|
+
def _initialize_remote_api(channel: grpc.Channel, cell: PrototypeCellConfig) -> None:
|
|
495
|
+
"""Register the remote planning subsystem before ``ConfigurePlanner``.
|
|
496
|
+
|
|
497
|
+
A fresh server holds no ``PlanningService`` until ``GriipApiAdmin.Initialize``
|
|
498
|
+
runs; until then ``ConfigurePlanner``/``PlanPath`` abort with ``UNAVAILABLE``.
|
|
499
|
+
Sends a part-free, perception-mocked Initialize that builds the planner and
|
|
500
|
+
registers the robot with VRMCS.
|
|
501
|
+
|
|
502
|
+
Idempotent. Initialize replaces the server's service registry, so calibration
|
|
503
|
+
and picking sessions must not share one server concurrently.
|
|
504
|
+
"""
|
|
505
|
+
admin_stub = griip_pb2_grpc.GriipApiAdminStub(channel)
|
|
506
|
+
send_initialize(admin_stub, build_robot_initialize_request(cell.model_dump(mode="python")))
|
|
507
|
+
logger.info("Remote mmai-griip-api initialized for hand-eye (robot registered; server-side perception mocked)")
|
|
508
|
+
|
|
509
|
+
|
|
510
|
+
def _configure_remote_planner(
|
|
511
|
+
planning_stub: PlanningStub,
|
|
512
|
+
cell: PrototypeCellConfig,
|
|
513
|
+
cell_config_directory: Path,
|
|
514
|
+
*,
|
|
515
|
+
cuboids_path_override: str | None = None,
|
|
516
|
+
) -> None:
|
|
517
|
+
"""Send the hand-eye :class:`pb.ConfigurePlannerRequest` over a gRPC stub.
|
|
518
|
+
|
|
519
|
+
Shares :func:`resolve_hand_eye_planner_params` with the in-process path so
|
|
520
|
+
both transports resolve gripper spheres (via the
|
|
521
|
+
:class:`~griip_sdk.cell.end_effector_collision_model.EndEffectorCollisionModel`)
|
|
522
|
+
and cuboids identically. The spheres are always sent inline.
|
|
523
|
+
"""
|
|
524
|
+
request_kwargs = resolve_hand_eye_planner_params(cell, cell_config_directory, cuboids_path_override=cuboids_path_override)
|
|
525
|
+
inline_spheres = request_kwargs.pop("gripper_collision_spheres")
|
|
526
|
+
request_kwargs["gripper_collision_spheres"] = [
|
|
527
|
+
pb.CollisionSpheres(center=pb.Vector3(x=sphere[0], y=sphere[1], z=sphere[2]), radius=sphere[3]) for sphere in inline_spheres
|
|
528
|
+
]
|
|
529
|
+
|
|
530
|
+
response = planning_stub.ConfigurePlanner(pb.ConfigurePlannerRequest(**request_kwargs))
|
|
531
|
+
logger.info(
|
|
532
|
+
"Hand-eye PlanningService configured over gRPC: environment_id=%s robot_dof=%s cuboids=%d",
|
|
533
|
+
response.environment_id,
|
|
534
|
+
response.robot_dof,
|
|
535
|
+
response.num_cuboids_loaded,
|
|
536
|
+
)
|
|
537
|
+
|
|
538
|
+
|
|
539
|
+
@contextmanager
|
|
540
|
+
def build_calibration_app(
|
|
541
|
+
cell_yaml: Path | str,
|
|
542
|
+
*,
|
|
543
|
+
calib_data: Path | str | None = None,
|
|
544
|
+
cuboids_path: Path | str | None = None,
|
|
545
|
+
mqtt_host: str | None = "localhost",
|
|
546
|
+
mqtt_port: int = 1883,
|
|
547
|
+
strict_intrinsics: bool = True,
|
|
548
|
+
) -> Iterator[CalibrationApp]:
|
|
549
|
+
"""Yield a wired :class:`CalibrationApp` for the cell at ``cell_yaml``.
|
|
550
|
+
|
|
551
|
+
Loads the config, builds the robot + camera clients, wires planning +
|
|
552
|
+
perception stubs (in-process for ``griip_api.mode: direct``; gRPC for
|
|
553
|
+
``grpc``), and builds the store. In gRPC mode it first issues a part-free
|
|
554
|
+
Initialize then ``ConfigurePlanner`` (without Initialize the latter aborts
|
|
555
|
+
with ``UNAVAILABLE``). On exit, releases the robot registration if this client
|
|
556
|
+
created it (failures logged) and
|
|
557
|
+
closes the channel. To inject custom stubs, construct :class:`CalibrationApp`
|
|
558
|
+
directly -- its ``__init__`` is keyword-only.
|
|
559
|
+
|
|
560
|
+
Args:
|
|
561
|
+
cell_yaml: path to the cell YAML.
|
|
562
|
+
calib_data: store root; defaults to :data:`DEFAULT_CALIBRATION_DATA_PATH`.
|
|
563
|
+
cuboids_path: override for the VAMP collision cuboids; defaults to
|
|
564
|
+
``cell.environment.cuboids_path``.
|
|
565
|
+
mqtt_host: broker host for live progress events; defaults to
|
|
566
|
+
``"localhost"``. Pass ``None`` to disable publishing. Hand-eye ticks
|
|
567
|
+
land on ``griip/calibration/hand_eye/progress``, environment ticks on
|
|
568
|
+
``griip/calibration/environment/progress``; the React components in
|
|
569
|
+
``@vention/machine-apps-components`` subscribe to both.
|
|
570
|
+
mqtt_port: broker port; defaults to ``1883``.
|
|
571
|
+
strict_intrinsics: when ``True`` (default), missing calibio intrinsics
|
|
572
|
+
raise :class:`FileNotFoundError`; ``False`` falls back to live
|
|
573
|
+
``camera_info`` (camera-swap escape hatch).
|
|
574
|
+
"""
|
|
575
|
+
resolved_path = Path(cell_yaml).expanduser().resolve()
|
|
576
|
+
cell = load_cell_config(resolved_path)
|
|
577
|
+
|
|
578
|
+
effective_cuboids_path = cell.environment.cuboids_path
|
|
579
|
+
if cuboids_path is not None:
|
|
580
|
+
effective_cuboids_path = str(Path(cuboids_path).expanduser().resolve())
|
|
581
|
+
logger.info("Using cuboids_path override: %s -> %s", cell.environment.cuboids_path, effective_cuboids_path)
|
|
582
|
+
|
|
583
|
+
logger.info("=== build_calibration_app ===")
|
|
584
|
+
logger.info("Config: %s griip_api.mode: %s", resolved_path, cell.griip_api.mode)
|
|
585
|
+
|
|
586
|
+
robot_client = build_vrmcs_client(resolved_path, cell)
|
|
587
|
+
grpc_channel: grpc.Channel | None = None
|
|
588
|
+
mqtt_publisher: MqttProgressPublisher | None = None
|
|
589
|
+
if mqtt_host is not None:
|
|
590
|
+
mqtt_publisher = MqttProgressPublisher(host=mqtt_host, port=mqtt_port)
|
|
591
|
+
try:
|
|
592
|
+
robot_client.reset_to_normal()
|
|
593
|
+
|
|
594
|
+
sensor = cell.camera.sensor
|
|
595
|
+
secondary_source_id = cell.camera.right_source_id
|
|
596
|
+
calib_root: Path | str = calib_data if calib_data is not None else DEFAULT_CALIBRATION_DATA_PATH
|
|
597
|
+
store = CalibrationConfigStore(root=calib_root)
|
|
598
|
+
|
|
599
|
+
# require_calibio=False: the app gates calibio per-verb via
|
|
600
|
+
# strict_intrinsics (see _resolve_calibration) instead of at build time.
|
|
601
|
+
camera = build_calibration_camera(cell.camera, store=store, require_calibio=False)
|
|
602
|
+
|
|
603
|
+
if cell.griip_api.mode == "grpc":
|
|
604
|
+
grpc_channel, planning_stub, perception_stub = _build_grpc_calibration_stubs(cell.griip_api.url)
|
|
605
|
+
logger.info("Connected to mmai-griip-api over gRPC at %s", cell.griip_api.url)
|
|
606
|
+
_initialize_remote_api(grpc_channel, cell)
|
|
607
|
+
_configure_remote_planner(planning_stub, cell, resolved_path.parent, cuboids_path_override=effective_cuboids_path)
|
|
608
|
+
else:
|
|
609
|
+
planning_stub = _build_in_process_planning_stub(
|
|
610
|
+
robot_client, cell, resolved_path.parent, cuboids_path_override=effective_cuboids_path
|
|
611
|
+
)
|
|
612
|
+
perception_stub = _build_in_process_perception_stub(cell)
|
|
613
|
+
|
|
614
|
+
yield CalibrationApp(
|
|
615
|
+
cell=cell,
|
|
616
|
+
config_path=resolved_path,
|
|
617
|
+
robot_client=robot_client,
|
|
618
|
+
vision_backend=camera.vision_backend,
|
|
619
|
+
cam_name=camera.camera_vendor,
|
|
620
|
+
camera_mxid=camera.camera_mx_id,
|
|
621
|
+
sensor=sensor,
|
|
622
|
+
secondary_source_id=secondary_source_id,
|
|
623
|
+
store=store,
|
|
624
|
+
calib_data_root=calib_root,
|
|
625
|
+
planning_stub=planning_stub,
|
|
626
|
+
perception_stub=perception_stub,
|
|
627
|
+
mqtt_publisher=mqtt_publisher,
|
|
628
|
+
strict_intrinsics=strict_intrinsics,
|
|
629
|
+
)
|
|
630
|
+
finally:
|
|
631
|
+
try:
|
|
632
|
+
robot_client.release_registration()
|
|
633
|
+
except Exception as exc:
|
|
634
|
+
logger.warning("release_registration: %s", exc)
|
|
635
|
+
if grpc_channel is not None:
|
|
636
|
+
grpc_channel.close()
|
|
637
|
+
if mqtt_publisher is not None:
|
|
638
|
+
mqtt_publisher.close()
|