diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 072e995c..8c5290b4 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -58,10 +58,16 @@ class DynamixelMotorsBus(MotorsBus): self.reader = dxl.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0) self.writer = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0) - def broadcast_ping(self) -> tuple[list, int]: - data_list, comm = self.packet_handler.broadcastPing(self.port_handler) - # TODO(aliberts): translate data_list into {id: model} - return data_list, comm + def broadcast_ping( + self, num_retry: int = 0, raise_on_error: bool = False + ) -> dict[int, list[int, int]] | None: + for _ in range(1 + num_retry): + data_list, comm = self.packet_handler.broadcastPing(self.port_handler) + if self._is_comm_success(comm): + return data_list + + if raise_on_error: + raise ConnectionError(f"Broadcast ping returned a {comm} comm error.") def calibrate_values(self, ids_values: dict[int, int]) -> dict[int, float]: # TODO diff --git a/tests/motors/test_dynamixel.py b/tests/motors/test_dynamixel.py index 3bccba1f..412a4a36 100644 --- a/tests/motors/test_dynamixel.py +++ b/tests/motors/test_dynamixel.py @@ -82,7 +82,46 @@ def test_abc_implementation(dummy_motors): """Instantiation should raise an error if the class doesn't implement abstract methods/properties.""" DynamixelMotorsBus(port="/dev/dummy-port", motors=dummy_motors) - DynamixelMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "xl330-m077")}) + +@pytest.mark.parametrize( + "idx, model_nb", + [ + [1, 1190], + [2, 1200], + [3, 1120], + ], +) +def test_ping(idx, model_nb, dummy_motors): + mock_motors = MockMotors() + mock_motors.build_ping_stub(idx, model_nb) + motors_bus = DynamixelMotorsBus( + port=mock_motors.port, + motors=dummy_motors, + ) + motors_bus.connect() + + ping_model_nb = motors_bus.ping(idx) + + assert ping_model_nb == model_nb + + +def test_broadcast_ping(dummy_motors): + expected_pings = { + 1: [1060, 50], + 2: [1120, 30], + 3: [1190, 10], + } + mock_motors = MockMotors() + mock_motors.build_broadcast_ping_stub(expected_pings) + motors_bus = DynamixelMotorsBus( + port=mock_motors.port, + motors=dummy_motors, + ) + motors_bus.connect() + + ping_list = motors_bus.broadcast_ping() + + assert ping_list == expected_pings @pytest.mark.parametrize(