fix(mcp): first call in 0.5 s: executor per node, tare over 1 s, UDP

mcp/Dockerfile

@@ -18,6 +18,12 @@ RUN apt-get update \
     && apt-get install -y --no-install-recommends "ros-${ROS_DISTRO}-rmw-cyclonedds-cpp" \
     && rm -rf /var/lib/apt/lists/*
 
+# Fast DDS reaches the other containers over UDP only. With its shared-memory
+# transport on, about one MCP restart in nine came up blind: the graph listed the
+# driver's topics but no message ever arrived (4 in 34 restarts on the bench, 0 in
+# 45 over UDP only, same idle CPU). CycloneDDS ignores the variable.
+ENV FASTDDS_BUILTIN_TRANSPORTS=UDPv4
+
 COPY --from=ghcr.io/astral-sh/uv:0.12.9 /uv /usr/local/bin/uv
 
 # The venv must be the system interpreter's, with system site-packages visible:

mcp/README.md

@@ -179,12 +179,28 @@ Then ask in plain language, RViz following along:
 > Open the driver gripper to 30 mm, then grasp with it and tell me what
 > happened.
 
-The grasp closes on nothing (`closed_without_object`), because fake hardware
-moves instantly and never meets resistance. A stall on an object, and with it
-`gripper_verify_grasp`, needs a real gripper with TSF-85 pads; the driver on the SDK's simulated gripper will cover the
-stall once that gripper models travel and object detection. The server's own
-instructions tell the agent how to read those outcomes, so a stalled close is
-reported as a grasp, not a failure.
+The close comes back `reached` with `object_detected: false`: fake hardware
+moves instantly and never meets resistance, so the fingers always arrive. A
+stall on an object (`stopped_on_object`), and with it `gripper_verify_grasp`,
+needs a real gripper with TSF-85 pads; the driver on the SDK's simulated
+gripper will cover the stall once that gripper models travel and object
+detection. The server's own instructions tell the agent how to read those
+outcomes, so a stalled close is reported as a grasp, not a failure.
+
+## Known limitations
+
+- **Health lags a dead driver by a few seconds.** When the driver stops,
+  `gripper_get_health` keeps reporting the controller as `ready` until ROS
+  notices it is gone. A move sent in that window fails with a timeout rather
+  than a clear "driver down".
+- **About one CPU core at idle on a gripper with pads.** The server reads every
+  `joint_states` message (500 Hz) and every pad frame (about 2 kHz) in Python,
+  which costs about 85 % of a core on the bench and about 30 % without pads.
+  Lowering those publish rates on the driver side is the way to bring it down.
+- **The achieved position is taken before the fingers settle.** The controller
+  reports a move done once it is within 2.1 mm of the goal, so a full close can
+  report 1.87 mm while the fingers end at 0.75 mm. Read the position again
+  after a short pause if you need the settled value.
 
 ## Development
 

mcp/demo/grippers.yaml

@@ -1,7 +1,7 @@
 # Wiring for the demo stack: one 2F-85 on the real driver, running
 # ros2_control's fake hardware at the root of the graph (hence namespace: /).
-# Fake hardware reports exactly the angle it was commanded, so a grasp closes to
-# 0 mm and reports closed_without_object. A real gripper stops short on an object
+# Fake hardware reports exactly the angle it was commanded, so a close reaches
+# 0 mm and reports `reached`, no object. A real gripper stops short on an object
 # (and reports stalled on every goal, see #29); the tactile tools need real pads.
 
 - name: driver

mcp/gripper_mcp/config.py

@@ -73,7 +73,7 @@ class TactileSpec(StrictModel):
     full_scale_counts: float = Field(gt=0.0)
     contact_threshold: float
     noise_margin: float = Field(ge=1.0)
-    baseline_samples: int = Field(gt=0)
+    baseline_s: float = Field(gt=0.0)
 
     @property
     def layout(self) -> TactileLayout:

mcp/gripper_mcp/datasheets/tactile/robotiq_tsf_85.yaml

@@ -28,8 +28,9 @@ contact_threshold: 0.02
 # hold reads about 0.8, so 2x the noise separates the two with room.
 noise_margin: 2.0
 
-# Distinct frames averaged by a tare. The SDK's findBaseline() takes 1000, but
-# measured in the #66 live run, about 110 frames a second reach Python whatever
-# the pads publish, so 1000 is a 9 s tool call; 100 is about 1 s and the rest
-# noise is +-2 counts.
-baseline_samples: 100
+# Seconds of frames a tare averages, however fast the pads publish. The pads
+# publish at about 1.7 kHz, so 1 s is about the 1000 samples the SDK's
+# findBaseline() takes. A shorter window misses the slow part of the rest
+# noise: on a free TSF-85, 0.06 s of frames gave 19 false contacts in 50
+# reads, 1 s gave none.
+baseline_s: 1.0

mcp/gripper_mcp/ros_backend.py

@@ -2,8 +2,13 @@
 
 One node per gripper, created inside the gripper's namespace so that
 `robotiq_gripper_controller/gripper_cmd`, `joint_states` and `robot_description`
-resolve to that gripper's controller. All nodes share one executor spinning in a
-background thread; the MCP tools run in their own threads and wait on futures.
+resolve to that gripper's controller. Each node spins on its own single-threaded
+executor in a background thread; the MCP tools run in their own threads and
+wait on futures. Single-threaded because rclpy's MultiThreadedExecutor takes a
+whole core to follow the driver's 500 Hz joint_states, still drops most of them,
+and starves the tool threads of the GIL. One per node because an executor serves
+its nodes in no fixed order, and the pads' 2 kHz frames sharing one would, on some
+starts, keep the gripper's own topics from ever being read.
 
 The command joint's name and range are read from `robot_description`, the URDF
 robot_state_publisher latches for the cell, so a prefixed cell resolves by itself
@@ -44,7 +49,7 @@
 from control_msgs.action import GripperCommand, ParallelGripperCommand
 from rclpy.action import ActionClient
 from rclpy.action.graph import get_action_names_and_types
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.executors import SingleThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import DurabilityPolicy, QoSProfile
 from sensor_msgs.msg import JointState
@@ -81,27 +86,33 @@
 
 class RosGraph:
     _lock = threading.Lock()
-    _executor: MultiThreadedExecutor | None = None
+    _executors: list[SingleThreadedExecutor] = []
 
     @classmethod
-    def executor(cls) -> MultiThreadedExecutor:
+    def node(cls, name: str, namespace: str) -> Node:
         with cls._lock:
-            if cls._executor is None:
-                if not rclpy.ok():
-                    rclpy.init()
-                cls._executor = MultiThreadedExecutor()
-                threading.Thread(
-                    target=cls._executor.spin, name="rclpy-spin", daemon=True
-                ).start()
-            return cls._executor
+            if not rclpy.ok():
+                rclpy.init()
+        return Node(name, namespace=namespace or "/")
+
+    @classmethod
+    def spin(cls, node: Node) -> None:
+        executor = SingleThreadedExecutor()
+        executor.add_node(node)
+        with cls._lock:
+            cls._executors.append(executor)
+        threading.Thread(
+            target=executor.spin, name=f"rclpy-spin-{node.get_name()}", daemon=True
+        ).start()
 
     @classmethod
     def shutdown(cls) -> None:
         with cls._lock:
-            if cls._executor is not None:
-                cls._executor.shutdown()
+            for executor in cls._executors:
+                executor.shutdown()
+            if cls._executors:
                 rclpy.shutdown()
-                cls._executor = None
+            cls._executors = []
 
 
 def wait_for(future, timeout_s: float):
@@ -165,8 +176,7 @@ class RosGripperBackend:
     name = "ros"
 
     def __init__(self, gripper_name: str, namespace: str) -> None:
-        executor = RosGraph.executor()
-        self._node = Node(f"gripper_mcp_{gripper_name}", namespace=namespace or "/")
+        self._node = RosGraph.node(f"gripper_mcp_{gripper_name}", namespace)
         self._description: str | None = None
         self._node.create_subscription(
             String, DESCRIPTION_TOPIC, self._on_description, LATCHED
@@ -177,7 +187,7 @@ def __init__(self, gripper_name: str, namespace: str) -> None:
         self._action_type = None
         self._client_lock = threading.Lock()
         self._motion_lock = threading.Lock()
-        executor.add_node(self._node)
+        RosGraph.spin(self._node)
 
     def joint_geometry(self) -> JointGeometry:
         if self._joint is None:

mcp/gripper_mcp/ros_tactile_backend.py

@@ -2,8 +2,8 @@
 
 One node per gripper, in the namespace the wiring gives the pads (the gripper's
 own unless `tactile_namespace` says otherwise), so `TactileSensor/StaticData`
-resolves to the pads on that gripper. It shares the executor thread with the
-gripper backends (RosGraph).
+resolves to the pads on that gripper. It spins on its own executor thread
+(RosGraph), so its 2 kHz frames never hold up the gripper backend's topics.
 
 The driver publishes with the sensor-data QoS (best effort), so the
 subscription must too: a reliable subscriber never matches a best-effort
@@ -16,18 +16,16 @@
 driver that stopped publishing would keep reporting its last frame forever,
 and a frozen "no contact" reads as "keep closing" to a tactile-guided close.
 
-`sample` is the exception: it asks the subscription callback to keep the next
-`count` messages and sleeps until the callback signals the buffer is full, so
-each frame is a distinct sensor update and the two threads hand off once per
-sample rather than once per frame (a per-frame handoff under the GIL was
-measured at under 100 frames/s against a 2 kHz publisher). It gives up with a
-stalled-sample error if no frame arrives for `STALE_FRAME_S` mid-sample.
+`sample` is the exception: the subscription callback keeps every frame that
+arrives during the tare window while the caller sleeps through it, so each
+frame is a distinct sensor update and the two threads never hand off per
+frame. The window is a duration rather than a frame count because the frame
+rate is the driver's to choose. A window that received no frame at all means
+the driver has stopped publishing.
 """
 
-import threading
 import time
 
-from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
 from robotiq_tsf.msg import StaticData
 
@@ -51,19 +49,15 @@ def __init__(
         self, gripper_name: str, namespace: str, layout: TactileLayout
     ) -> None:
         self._layout = layout
-        executor = RosGraph.executor()
-        self._node = Node(
-            f"gripper_mcp_tactile_{gripper_name}", namespace=namespace or "/"
-        )
+        self._node = RosGraph.node(f"gripper_mcp_tactile_{gripper_name}", namespace)
         self._static: StaticData | None = None
         self._latest_at = 0.0
-        self._wanted = 0
+        self._collecting = False
         self._collected: list[StaticData] = []
-        self._sample_done = threading.Event()
         self._node.create_subscription(
             StaticData, STATIC_TOPIC, self._on_static, qos_profile_sensor_data
         )
-        executor.add_node(self._node)
+        RosGraph.spin(self._node)
 
     def read_tactile(self) -> TactileReading:
         static = poll_until(lambda: self._static, STATIC_WAIT_S)
@@ -77,19 +71,15 @@ def read_tactile(self) -> TactileReading:
             raise RuntimeError(stale_frame_message(age_s, STALE_FRAME_S))
         return self._reading(static)
 
-    def sample(self, count: int) -> list[TactileReading]:
-        self._wanted = 0
+    def sample(self, duration_s: float) -> list[TactileReading]:
         self._collected = []
-        self._sample_done.clear()
-        self._wanted = count
-        seen = 0
-        while not self._sample_done.wait(STALE_FRAME_S):
-            if len(self._collected) == seen:
-                self._wanted = 0
-                raise RuntimeError(stalled_sample_message(seen, count, STALE_FRAME_S))
-            seen = len(self._collected)
-        self._wanted = 0
-        return [self._reading(message) for message in self._collected]
+        self._collecting = True
+        time.sleep(duration_s)
+        self._collecting = False
+        frames = list(self._collected)
+        if not frames:
+            raise RuntimeError(stalled_sample_message(duration_s))
+        return [self._reading(message) for message in frames]
 
     def _reading(self, static: StaticData) -> TactileReading:
         return reading_from_counts(
@@ -99,7 +89,5 @@ def _reading(self, static: StaticData) -> TactileReading:
     def _on_static(self, message: StaticData) -> None:
         self._static = message
         self._latest_at = time.monotonic()
-        if len(self._collected) < self._wanted:
+        if self._collecting:
             self._collected.append(message)
-            if len(self._collected) == self._wanted:
-                self._sample_done.set()

mcp/gripper_mcp/ros_tactile_messages.py

@@ -46,8 +46,8 @@ def stale_frame_message(age_s: float, limit_s: float) -> str:
     )
 
 
-def stalled_sample_message(collected: int, wanted: int, limit_s: float) -> str:
+def stalled_sample_message(window_s: float) -> str:
     return (
-        f"No new {STATIC_TOPIC} frame for {limit_s:.0f} s after {collected} of "
-        f"{wanted} samples; the robotiq_tsf driver has stopped publishing."
+        f"No {STATIC_TOPIC} frame during the {window_s:g} s tare window; "
+        "the robotiq_tsf driver has stopped publishing."
     )

mcp/gripper_mcp/tactile_backend.py

@@ -49,4 +49,4 @@ class TactileBackend(Protocol):
 
     def read_tactile(self) -> TactileReading: ...
 
-    def sample(self, count: int) -> list[TactileReading]: ...
+    def sample(self, duration_s: float) -> list[TactileReading]: ...

mcp/gripper_mcp/tactile_service.py

@@ -63,6 +63,7 @@ class Tare:
     baseline: TactileBaseline
     noise_floor: float
     threshold: float
+    samples: int
 
 
 class TactileService:
@@ -82,7 +83,7 @@ def tare(self, gripper_name: str) -> TactileTareResult:
 
         return TactileTareResult(
             gripper_name=gripper_name,
-            samples=spec.baseline_samples,
+            samples=tare.samples,
             rest_counts_mean=round(mean_counts(tare.baseline), 2),
             noise_floor=round(tare.noise_floor, 5),
             threshold=round(tare.threshold, 5),
@@ -146,7 +147,7 @@ def _capture_tare(
         self, gripper_name: str, tactile: TactileBackend, spec: TactileSpec
     ) -> Tare:
         with self._tare_lock:
-            tare = measure_tare(tactile.sample(spec.baseline_samples), spec)
+            tare = measure_tare(tactile.sample(spec.baseline_s), spec)
             self._tares[gripper_name] = tare
         return tare
 
@@ -184,6 +185,7 @@ def measure_tare(readings: list[TactileReading], spec: TactileSpec) -> Tare:
         baseline=baseline,
         noise_floor=noise_floor,
         threshold=max(spec.contact_threshold, spec.noise_margin * noise_floor),
+        samples=len(readings),
     )
 
 

mcp/tests/fakes/tactile.py

@@ -31,6 +31,7 @@
 TOUCH_COUNTS = 20
 TAXEL_MAX_COUNTS = 110
 STIFFNESS_COUNTS_PER_MM = 6.9
+FRAME_RATE_HZ = 100
 
 
 class MockTactileBackend:
@@ -60,8 +61,8 @@ def read_tactile(self) -> TactileReading:
             layout=self._layout,
         )
 
-    def sample(self, count: int) -> list[TactileReading]:
-        return [self.read_tactile() for _ in range(count)]
+    def sample(self, duration_s: float) -> list[TactileReading]:
+        return [self.read_tactile() for _ in range(round(duration_s * FRAME_RATE_HZ))]
 
     def _taxel_counts(self) -> int:
         penetration_mm = self._penetration_mm()

mcp/tests/test_config.py

@@ -218,15 +218,15 @@ def test_a_missing_wiring_file_names_the_path(tmp_path):
         load_gripper_configs(missing)
 
 
-def test_a_tactile_datasheet_with_no_samples_to_average_is_rejected(tmp_path):
+def test_a_tactile_datasheet_with_no_tare_window_is_rejected(tmp_path):
     sheet = tmp_path / "robotiq_tsf_85.yaml"
     sheet.write_text(
         (TACTILE_SPEC_DIR / "robotiq_tsf_85.yaml")
         .read_text()
-        .replace("baseline_samples: 100", "baseline_samples: 0")
+        .replace("baseline_s: 1.0", "baseline_s: 0")
     )
 
-    with pytest.raises(ValidationError, match="baseline_samples"):
+    with pytest.raises(ValidationError, match="baseline_s"):
         load_tactile_spec(sheet)
 
 

mcp/tests/test_mock_tactile_backend.py

@@ -1,4 +1,5 @@
 from fakes.tactile import (
+    FRAME_RATE_HZ,
     REST_COUNTS,
     TAXEL_MAX_COUNTS,
     TOUCH_COUNTS,
@@ -13,7 +14,7 @@
 LIGHTLY_PRESSED_MM = 38.0
 FIRMLY_PRESSED_MM = 30.0
 CRUSHED_MM = 0.0
-SAMPLE_COUNT = 3
+SAMPLE_WINDOW_S = 0.03
 
 
 def backend(opening_mm: float, object_width_mm: float | None) -> MockTactileBackend:
@@ -100,10 +101,10 @@ def test_a_sample_is_one_fresh_reading_per_frame_asked():
         read_opening_mm=lambda: opening["mm"], object_width_mm=OBJECT_WIDTH_MM
     )
 
-    resting = live.sample(SAMPLE_COUNT)
+    resting = live.sample(SAMPLE_WINDOW_S)
     opening["mm"] = FIRMLY_PRESSED_MM
-    pressed = live.sample(SAMPLE_COUNT)
+    pressed = live.sample(SAMPLE_WINDOW_S)
 
-    assert len(resting) == len(pressed) == SAMPLE_COUNT
+    assert len(resting) == len(pressed) == round(SAMPLE_WINDOW_S * FRAME_RATE_HZ)
     assert all(all_resting(frame) for frame in resting)
     assert not any(all_resting(frame) for frame in pressed)

mcp/tests/test_ros_tactile_messages.py

@@ -40,8 +40,8 @@ def test_a_stale_frame_message_names_the_age_and_the_driver():
     assert "stopped publishing" in message
 
 
-def test_a_stalled_sample_says_how_far_it_got():
-    message = stalled_sample_message(42, 1000, 2.0)
+def test_a_stalled_sample_names_the_window_it_waited():
+    message = stalled_sample_message(1.0)
 
-    assert "42 of 1000" in message
+    assert "1 s tare window" in message
     assert STATIC_TOPIC in message