fix(mcp): first call in 0.5 s: executor per node, tare over 1 s, UDP - #93
jlongvalRobotiq wants to merge 6 commits into
Conversation
A count only means a duration at one frame rate, and the frame rate is the driver's to choose. #83 set 100 frames because about 110 frames a second reached Python whatever the pads published; that ceiling was the rclpy executor, not the pads. The TSF-85 publishes at about 2 kHz, so once every frame arrives, 100 frames are 0.06 s of rest signal: too short to see the slow part of the rest noise, and the threshold set from it sits under that noise. On a free TSF-85 on the bench that gave 19 false contacts in 50 reads. The datasheet now gives `baseline_s: 1.0`, Eric's 1 s compromise from #66, and the tare averages every frame received in that window (about 2000 at the pads' rate, 0 false contacts in 50 reads, pad on pad still read as contact at full close). The result's `samples` reports how many frames that was. The ROS source keeps every frame arriving during the window while the caller sleeps through it, and a window that received none is a stopped driver. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
The driver publishes joint_states at 500 Hz and the TSF-85 driver its frames at about 2 kHz. rclpy's MultiThreadedExecutor took a whole core to follow that and still dropped about four messages in five (measured: 108 % CPU, 86 to 105 of 500 joint_states a second received; a SingleThreadedExecutor on the same topic: 22 %, all 500). The spin thread held the GIL so constantly that the server's first tool call, which does its one-time imports and schema work then, took 12 to 18 s: longer than the 10 s motion timeout, so the first move after the MCP starts could come back incomplete. Every later call paid it too, 0.34 s where 0.11 s is enough. One executor per node rather than one shared: an executor serves its nodes in no fixed order, and sharing one with the pads' 2 kHz frames left the gripper's own topics unread on some starts. Each node is added before its executor spins, so nothing is added to a running executor. None of the callbacks blocks: they store a message, set an event or send a cancel, and the tools wait on their own threads. Demo (fake hardware): first call after an MCP restart 12.2 to 16.7 s -> 0.52 to 0.57 s, later calls 0.34 -> 0.11 s, idle CPU 108 -> 30 %. Bench (real 2F-85 + TSF-85): first move after an MCP restart 13 to 18 s -> 1.25 to 1.75 s; 20 open/close cycles, 40 verdicts right. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
With Fast DDS's shared-memory transport on, about one MCP restart in nine came up blind: the ROS graph listed the driver's topics and action server, but no joint_states, robot_description or tactile frame ever arrived, so every tool failed with "No robot_description within 2 s" until the next restart. On the bench (all containers on host network and host IPC): 4 blind starts in 34 restarts with shared memory, 0 in 65 over UDP only, same idle CPU. The variable is ignored by CycloneDDS, which the cell runs. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
Health lagging a dead driver, the idle CPU cost of following 500 Hz and 2 kHz topics in Python, and a position reported before the fingers settle all surprise a first user and none shows on the fake-hardware demo. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
1fa86a5 to
6289e37
Compare
e03ae55 to
deb4cde
Compare
|
I think that many of these problems would have been avoided if the MCP had been written in c++, based only on the SDKs. Is there a use case that requires the ROS layer ? |
There was a problem hiding this comment.
Approve as a stopgap. The executor-per-node design is sound and the threading is correct; the tare rewrite is simpler than what it replaces; the UDP-only transport is the right fix for a container that only talks to other containers (both compose files already share ipc: host, so the shared-memory failure is not a missing IPC namespace). Bench claims in the README cross-check against the repo: update_rate: 500 in robotiq_controllers.yaml, goal_tolerance: 0.02 rad ≈ 2.1 mm on an 85 mm / ~0.8 rad stroke, and the SDK's findBaseline() does collect 1000 samples. The PR title's "0.5 s" is the fake-hardware figure; the body says 1.25–1.75 s on the bench, which is the number that matters to a user.
Findings and nits are inline.
Shelved (pre-existing, not for this PR)
RosGraph.shutdownnever callsdestroy_node()on the nodes; harmless at process exit, worth a line if the server ever reloads its wiring.TactileService._tare_lockis one lock for every gripper, so a 1 s tare on one gripper blocks reads that need a tare on another. Was true at 100 frames too, just shorter.- The ROS backends and
RosGraphhave no tests (rclpy not importable in the unit suite). Thesamplerewrite therefore ships covered only through the fakes. Consistent with the rest ofmcp/; note it, no action here.
Commit 3cef4356 (revert of #83) followed by f467e5d7 that rewrites the same lines: fine as history, but the revert message could point at f467e5d7's reason in one line so git log on the yaml makes sense without the PR.
| # 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 |
There was a problem hiding this comment.
so 1 s is about the 1000 samples
->
so 1 s is more than the 1000 samples
| 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)) |
There was a problem hiding this comment.
1. Design — one more reason to move the tare to the SDK, as already agreed
The team already agreed to replace the MCP's tare with the tactile SDK's RobotiqTactileSensor::findBaseline() (tactile_sensors/sdk_cpp/RobotiqTactileSensor.h:110, 1000 samples at the sensor's own rate). This PR is evidence for why: it is the second round of tuning a tare implemented in Python over ROS frames (#83 set a frame count, this one a duration), and each round chased a property of the transport (the executor's frame ceiling, then the real frame rate) rather than of the pads. The new code still has a hole of the same kind: a window that got only a few frames is accepted, so a stalled stream yields a near-zero noise floor and the false contacts return silently. Fixing that would be a third round on code that is going away.
Take this commit as the stopgap it is, no further changes, and let the false-contact history (19 in 50 before #83, this fix, the stalled-window case) go into the migration issue as its motivation. If the ROS backend outlives the migration, the interim route is a tare service on the robotiq_tsf driver that wraps findBaseline(), as the tactile_centering demo does through /tactile_preprocessing/reset_baseline.
| # 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 |
There was a problem hiding this comment.
2. Low — FASTDDS_BUILTIN_TRANSPORTS is silently ignored on Humble
The variable exists from Fast DDS 2.10 (Iron and later). Humble ships 2.6, which does not read it, so a Humble base image (the README says Humble is supported and CI tests its interpreter) keeps shared memory on and keeps the one-in-nine blind start. Either say so in the comment and tag it per the repo rule (Humble EOL: drop this note), or give Humble the XML route (FASTRTPS_DEFAULT_PROFILES_FILE pointing at a profile with <transport_descriptors> UDPv4 only, plus RMW_FASTRTPS_USE_QOS_FROM_XML not needed for transports). A one-line note is enough if Humble is not a target for the MCP image; then the README should say the image is Jazzy-only.
| # 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. |
There was a problem hiding this comment.
3. Low — bench measurements in code comments
The Dockerfile comment carries "4 in 34 restarts, 0 in 45", the PR body says "0 in 65" for the same claim; the yaml says the pads publish at 1.7 kHz, the three other comments say 2 kHz. Numbers that live in two places drift. Keep the why in the comment (shared memory came up blind across containers; a short window misses the slow drift) and leave the counts in the PR body, which is where the rest of the QA evidence already is. The ros_backend.py module docstring is fine as the design rationale but "takes a whole core ... still drops most of them" is the same kind of measurement; "cannot keep up with 500 Hz joint_states under the GIL" says the why without a figure that will go stale.
| @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 = [] |
There was a problem hiding this comment.
4. Low — RosGraph.node can leave rclpy initialised with nothing to shut down
node() calls rclpy.init(); shutdown() calls rclpy.shutdown() only if an executor was registered. If a backend constructor raises between node() and spin() (bad namespace, subscription type import failure) rclpy stays up with no owner. Trivial fix: track an _initialised flag set in node() and test that instead of if cls._executors, or create the executor and register it in node() and only start the thread in spin().
| @@ -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 | |||
There was a problem hiding this comment.
Nit: mcp/tests/test_mock_tactile_backend.py:96 — test name test_a_sample_is_one_fresh_reading_per_frame_asked no longer describes a duration-based sample; ..._per_frame_in_the_window.
| TOUCH_COUNTS = 20 | ||
| TAXEL_MAX_COUNTS = 110 | ||
| STIFFNESS_COUNTS_PER_MM = 6.9 | ||
| FRAME_RATE_HZ = 100 |
There was a problem hiding this comment.
Nit: mcp/tests/fakes/tactile.py:34 — FRAME_RATE_HZ = 100 is a fake's rate used by three test files to compute expected counts; a one-line comment that it is arbitrary and only has to make baseline_s * rate an integer would stop someone "correcting" it to 2000.
| 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. | ||
| """ |
There was a problem hiding this comment.
Nit: mcp/gripper_mcp/ros_tactile_backend.py:20-25 — module docstring: "the two threads never hand off per frame" restates what the code shows (_collecting flag, time.sleep); the non-obvious part is only "a duration rather than a count because the frame rate is the driver's to choose". Trim to that.
| - **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. |
There was a problem hiding this comment.
Nit: mcp/README.md:195-198 — "about 85 % of a core" here vs "about one CPU core" in the PR body; pick one wording and reuse it.
The demo section of the README and the demo wiring said a close on fake hardware reports closed_without_object, an outcome the server no longer has since the motion tools moved to neutral outcomes (27d52b0). It comes back reached, with object_detected false. Co-Authored-By: Claude Opus 5.5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
2 participants
Why this PR
Before merging
feat/mcpintomain, we ran a full QA pass of the gripper MCP server by hand, on the bench: a real 2F-85 with TSF-85 pads, with Claude Code as the client, as an agent would use it. Every tool and every failure case got one test, and each test's expected result was written down before the run. The pass found the problems fixed here, plus one fixed separately in #91. With both PRs, every test passed.What we tested
The same tests also ran on the fake-hardware demo, including the ones with the driver stopped.
What was wrong, and the fix
1. The first move after the server started took 13 to 18 s, longer than the 10 s move limit, so an agent's very first move could fail. The server could not keep up with the ROS messages coming in: finger positions 500 times a second, pad readings about 2,000 times a second. The ROS message loop it used (
MultiThreadedExecutor) took a whole CPU core and still dropped about four messages in five, and it slowed every other part of the server down. Each ROS node now gets its own simple, single-thread loop (SingleThreadedExecutor).One shared loop for all nodes was not enough: on some starts, the pad messages crowded out the gripper's own and the gripper was never read.
2. The pads reported contact when nothing touched them, 19 times in 50. Zeroing the pads (the "tare") averaged 100 readings, a number set in #83 when only about 110 readings a second got through. Once every reading gets through, 100 readings is only 0.06 s, too short to average out the pads' slow drift at rest. #83 is reverted, and the tare now averages everything that arrives in 1 s (about 2,000 readings), the 1 s Eric suggested in #66.
3. About one server restart in nine, the server saw no messages at all, and every tool failed until the next restart. This came from Fast DDS's shared-memory transport. The MCP image now sets
FASTDDS_BUILTIN_TRANSPORTS=UDPv4so it uses plain network messages. CycloneDDS, which the cell runs, ignores this setting.4. The README lists the known limitations seen on the bench (below).
Verified
uv run pytest: 198 passed; black, flake8 and the quality gate pass on every commit.Needs
Known limitations (in the README)
Still to come on this PR