diff --git a/mcp/gripper_mcp/ros_messages.py b/mcp/gripper_mcp/ros_messages.py index abfd590..74fc543 100644 --- a/mcp/gripper_mcp/ros_messages.py +++ b/mcp/gripper_mcp/ros_messages.py @@ -105,6 +105,8 @@ def advertised_type(names_and_types, action_name: str, known: dict) -> str | Non def position_of(state, joint: str, fallback: float) -> float: names = list(state.name) + if not names and len(state.position) == 1: + return state.position[0] if joint not in names: return fallback return state.position[names.index(joint)] diff --git a/mcp/gripper_mcp/service.py b/mcp/gripper_mcp/service.py index d3b2871..2670377 100644 --- a/mcp/gripper_mcp/service.py +++ b/mcp/gripper_mcp/service.py @@ -219,6 +219,8 @@ def classify( return "refused" if motion.timed_out: return "incomplete" + if motion.reached_goal: + return "reached" if stopped_on_something(commanded_mm, achieved_mm, stroke): return "stopped_on_object" return "reached" diff --git a/mcp/tests/test_ros_messages.py b/mcp/tests/test_ros_messages.py index 8a68517..d0c31ee 100644 --- a/mcp/tests/test_ros_messages.py +++ b/mcp/tests/test_ros_messages.py @@ -116,6 +116,18 @@ def test_a_state_without_the_joint_yields_the_fallback(): assert position_of(state, "knuckle_joint", FALLBACK_RAD) == FALLBACK_RAD +def test_a_result_naming_no_joint_but_carrying_one_position_is_that_joint(): + state = FakeState([], [KNUCKLE_RAD]) + + assert position_of(state, "knuckle_joint", FALLBACK_RAD) == KNUCKLE_RAD + + +def test_a_result_naming_no_joint_with_no_position_yields_the_fallback(): + state = FakeState([], []) + + assert position_of(state, "knuckle_joint", FALLBACK_RAD) == FALLBACK_RAD + + class FakeCancelResponse: def __init__(self, return_code): self.return_code = return_code diff --git a/mcp/tests/test_service.py b/mcp/tests/test_service.py index d930417..2770224 100644 --- a/mcp/tests/test_service.py +++ b/mcp/tests/test_service.py @@ -223,6 +223,7 @@ def test_each_gripper_opens_to_its_own_model_width(): (motion(reached_goal=False, stalled=True), 85.0, 49.0, "stopped_on_object"), (motion(reached_goal=True), 30.0, 30.0, "reached"), (motion(reached_goal=False, stalled=True), 30.0, 30.0, "reached"), + (motion(reached_goal=True), 85.0, 83.1, "reached"), ], ids=[ "refused", @@ -233,6 +234,7 @@ def test_each_gripper_opens_to_its_own_model_width(): "open blocked part-way", "move landed", "move landed but the driver says stalled (#29)", + "the controller reached within its own tolerance, wider than ours", ], ) def test_classify_decides_from_position_not_the_stall_flag(