async def test_set_position_pids(socket_adapter_mock, configuration_test): """ Test that the motor gateway encodes a pid configuration message, with float values. Assumes that speed PIDs are initially 0, if never set before. """ motor_gateway = MotorGateway(socket_adapter_mock, configuration_test) await motor_gateway.set_pid_position(1 * pi, 2 * pi, 3 * pi, 4 * pi, 5 * pi, 6 * pi) packet, _ = socket_adapter_mock.send.call_args message = BusMessage() message.ParseFromString(*packet) left_pid = PIDCoefficients( kp=1 * pi, ki=2 * pi, kd=3 * pi, ) right_pid = PIDCoefficients( kp=4 * pi, ki=5 * pi, kd=6 * pi, ) # If we want to set the position PIDs, speed PIDs should be set to 0 assert message == BusMessage(pidConfig=PIDConfigMsg( pid_speed_left=PIDCoefficients(kp=0, ki=0, kd=0), pid_speed_right=PIDCoefficients(kp=0, ki=0, kd=0), pid_position_left=left_pid, pid_position_right=right_pid))
async def test_dispatch_debug_log(protobuf_router): """ Test low level log message. """ bus_message = BusMessage(debugLog=DebugLog(content='Hello, world!')) msg_bytes = bus_message.SerializeToString() await protobuf_router.decode_message(msg_bytes, 'motor_board')
async def movement_done(self) -> None: """ Send the "movement done" signal to the robot. """ bus_message = BusMessage(movementEnded=MovementEndedMsg(blocked=False)) msg_bytes = bus_message.SerializeToString() await self.motor_board_adapter.send(msg_bytes)
async def test_move_rotation(simulation_handler, event_queue_mock): """ Happy path for rotation. """ bus_message = BusMessage(rotate=RotateMsg(ticks=3)) msg_bytes = bus_message.SerializeToString() await simulation_handler.handle_movement_order(msg_bytes) event_queue_mock.push.assert_any_call( EventOrder(type=EventType.MOVE_WHEEL, payload={ 'left': -1, 'right': 1 }), 62) event_queue_mock.push.assert_any_call( EventOrder(type=EventType.MOVE_WHEEL, payload={ 'left': -1, 'right': 1 }), 188) event_queue_mock.push.assert_any_call( EventOrder(type=EventType.MOVE_WHEEL, payload={ 'left': -1, 'right': 1 }), 313) event_queue_mock.push.assert_any_call( EventOrder(type=EventType.MOVEMENT_DONE, payload=None), 397) assert event_queue_mock.push.call_count == 4
async def translate(self, ticks: int) -> None: """ Move forward if ticks > 0, backward if ticks < 0. """ LOGGER.get().debug('gateway_translate', ticks=ticks) message = BusMessage(translate=TranslateMsg(ticks=ticks)) payload = message.SerializeToString() await self.motor_board_adapter.send(payload)
async def rotate(self, ticks: int) -> None: """ Rotate counter-clockwise if ticks > 0, clockwise if ticks < 0. """ LOGGER.get().debug('gateway_rotate', ticks=ticks) message = BusMessage(rotate=RotateMsg(ticks=ticks)) payload = message.SerializeToString() await self.motor_board_adapter.send(payload)
async def decode_message(self, msg: bytes, source: str) -> None: """ Convert bytes to BusMessage. """ bus_message = BusMessage() bus_message.ParseFromString(msg) printable_msg = json_format.MessageToJson( bus_message, including_default_value_fields=True) LOGGER.get().debug('msg_can', msg=printable_msg, source=source) await self.dispatch_message(bus_message, source)
async def test_movement_ended(protobuf_handler, localization_controller_mock, blocked): """ Route movement ended messages to localization controller. """ bus_message = BusMessage(movementEnded=MovementEndedMsg(blocked=blocked)) msg_bytes = bus_message.SerializeToString() await protobuf_handler.translate_message(msg_bytes) localization_controller_mock.movement_done.assert_called_once_with(blocked)
async def test_move_wheels_zero_unit(simulation_handler, event_queue_mock): """ If we receive a packet to move wheels by 0 unit, do nothing. """ bus_message = BusMessage(translate=TranslateMsg(ticks=0)) msg_bytes = bus_message.SerializeToString() await simulation_handler.handle_movement_order(msg_bytes) event_queue_mock.push.assert_called_with( EventOrder(type=EventType.MOVEMENT_DONE, payload=None), 0)
async def encoder_position(self, left_tick: int, right_tick: int) -> None: """ Send encoder positions. """ bus_message = BusMessage(encoderPosition=EncoderPositionMsg( left_tick=left_tick, right_tick=right_tick, )) msg_bytes = bus_message.SerializeToString() await self.motor_board_adapter.send(msg_bytes)
async def test_set_tolerances(socket_adapter_mock, configuration_test): """ Test that the motor gateway encodes a wheel tolerance message. """ motor_gateway = MotorGateway(socket_adapter_mock, configuration_test) await motor_gateway.set_tolerances(10, 20) packet, _ = socket_adapter_mock.send.call_args message = BusMessage() message.ParseFromString(*packet) assert message == BusMessage( wheelTolerances=WheelTolerancesMsg(ticks_left=10, ticks_right=20))
async def test_speed_order(simulation_router, simulation_state_mock): """ Happy path for translation. """ bus_message = BusMessage(moveWheelAtSpeed=MoveWheelAtSpeedMsg( left_tick_per_sec=100, right_tick_per_sec=200)) msg_bytes = bus_message.SerializeToString() await simulation_router.handle_movement_order(msg_bytes, 'test') assert simulation_state_mock.queue_speed_left[-1] == 100 assert simulation_state_mock.queue_speed_right[-1] == 200
async def test_set_speed(socket_adapter_mock, configuration_test): """ Test that the motor gateway encodes a wheel target speed message. """ motor_gateway = MotorGateway(motor_board_adapter=socket_adapter_mock, configuration=configuration_test) await motor_gateway.set_target_speeds(10, 20) packet, _ = socket_adapter_mock.send.call_args message = BusMessage() message.ParseFromString(*packet) assert message == BusMessage(moveWheelAtSpeed=MoveWheelAtSpeedMsg( left_tick_per_sec=10, right_tick_per_sec=20))
async def test_set_position(socket_adapter_mock, configuration_test): """ Test that the motor gateway encodes a wheel target positon message. """ motor_gateway = MotorGateway(socket_adapter_mock, configuration_test) await motor_gateway.set_target_positions(10, 20) packet, _ = socket_adapter_mock.send.call_args message = BusMessage() message.ParseFromString(*packet) assert message == BusMessage( wheelPositionTarget=WheelPositionTargetMsg(tick_left=10, tick_right=20))
async def test_dispatch_encoder_position(protobuf_router, position_controller_mock): """ Dispatch encoder position to position controller. """ bus_message = BusMessage(encoderPosition=EncoderPositionMsg( left_tick=1, right_tick=-2, )) msg_bytes = bus_message.SerializeToString() await protobuf_router.decode_message(msg_bytes, 'test') position_controller_mock.update_odometry.assert_called_once_with(1, -2)
async def test_dispatch_laser_sensor(protobuf_handler, match_action_controller_mock): """ If LaserSensorMsg provided, should call match_action_controller.set_laser_distances once. """ bus_message = BusMessage( laserSensor=LaserSensorMsg(distance_front_left=10, distance_front_right=10, distance_back_left=10, distance_back_right=10)) msg_bytes = bus_message.SerializeToString() await protobuf_handler.translate_message(msg_bytes) match_action_controller_mock.set_laser_distances.assert_called_once_with()
async def test_dispatch_encoder_position(protobuf_handler, localization_controller_mock): """ Dispatch encoder position to localization controller. """ bus_message = BusMessage(encoderPosition=EncoderPositionMsg( left_tick=1, right_tick=-2, )) msg_bytes = bus_message.SerializeToString() await protobuf_handler.translate_message(msg_bytes) localization_controller_mock.update_odometry_position.assert_called_once_with( 1, -2)
async def test_dispatch_pressure_sensor(protobuf_handler, match_action_controller_mock): """ If PressureSensorMsg provided, should call match_action_controller.set_pressures once. """ bus_message = BusMessage( pressureSensor=PressureSensorMsg(on_left=10, on_center_left=10, on_center=10, on_center_right=10, on_right=10)) msg_bytes = bus_message.SerializeToString() await protobuf_handler.translate_message(msg_bytes) match_action_controller_mock.set_pressures.assert_called_once_with()
async def callback(bytes, name): global t_last t = time.time() bus_message = BusMessage() try: bus_message.ParseFromString(bytes) printable_data = json_format.MessageToDict( bus_message, including_default_value_fields=True) json_data = json.dumps(printable_data) sys.stdout.write(f'{(t - t_last) * 1000:10.3f} ms:"{name}" ' + json_data + '\n') sys.stdout.flush() except DecodeError: print("Protobuf couldn't decode this:", bytes) t_last = t
async def dispatch_message(self, bus_message: BusMessage, source: str) -> None: """ Detect type of a BusMessage and call a controller. """ type_msg = bus_message.WhichOneof("message_content") if type_msg == "heartbeat": pass elif type_msg == "encoderPosition": self.position_controller.update_odometry( bus_message.encoderPosition.left_tick, bus_message.encoderPosition.right_tick) self.motion_controller.trigger_update() elif type_msg == "laserSensor": await self.match_action.set_laser_distances() elif type_msg == "pressureSensor": await self.match_action.set_pressures() elif type_msg == "debugLog": LOGGER.get().info("low_level_log", content=bus_message.debugLog.content, source=source) elif type_msg in [ "moveWheelAtSpeed", "wheelPositionTarget", "pidConfig", "wheelTolerances", "wheelPWM", "wheelControlMode" ]: # messages that are read back are ignored pass else: LOGGER.get().error("unhandled_protobuf_message", message_type=type_msg, source=source)
async def set_tolerances(self, ticks_left: int, ticks_right: int) -> None: """ Sets the motor board's tolerance to its wheel position relative to the targets, in ticks. """ LOGGER.get().debug('motor_gateway_set_tolerances', left=ticks_left, right=ticks_right) message = BusMessage(wheelTolerances=WheelTolerancesMsg( ticks_left=ticks_left, ticks_right=ticks_right)) await self._send_message(message)
async def set_pwms(self, ratio_left: float, ratio_right: float) -> None: """ Sets each wheel's PWM output. Ratios are duty cycles, signs are directions. """ LOGGER.get().info('motor_gateway_set_pwms', ratio_left=ratio_left, ratio_right=ratio_right) message = BusMessage(wheelPWM=WheelPWMMsg(ratio_left=ratio_left, ratio_right=ratio_right)) await self._send_message(message)
async def _send_message(self, adapter_index: int, message: BusMessage) -> None: """ Serializes and sends sends a protobuf BusMessage through an adapter. """ if adapter_index >= len(self._servo_board_adapters): raise RuntimeError( f"Board ID out of bounds:" f"{adapter_index}>={len(self._servo_board_adapters)}") payload = message.SerializeToString() await self._servo_board_adapters[adapter_index].send(payload)
async def set_target_positions(self, tick_left: Tick, tick_right: Tick) -> None: """ Sets each wheel's position target, in encoder ticks. """ LOGGER.get().debug('motor_gateway_set_target_positions', tick_left=tick_left, tick_right=tick_right) message = BusMessage(wheelPositionTarget=WheelPositionTargetMsg( tick_left=tick_left, tick_right=tick_right)) await self._send_message(message)
async def set_control_mode(self, speed: bool, position: bool) -> None: """ Sets a boolean in the motor board making it control either its wheel speeds or positions. Settings both generates an error and both are disabled. """ LOGGER.get().debug('motor_gateway_set_control_mode', speed_controlled=speed, position_controlled=position) message = BusMessage(wheelControlMode=WheelControlModeMsg( speed=speed, position=position)) await self._send_message(message)
async def set_target_speeds(self, left: TickPerSec, right: TickPerSec) -> None: """ Sets each wheel's speed target. """ LOGGER.get().debug('motor_gateway_set_target_speeds', left_speed=left, right_speed=right) message = BusMessage(moveWheelAtSpeed=MoveWheelAtSpeedMsg( left_tick_per_sec=round(left), right_tick_per_sec=round(right))) await self._send_message(message)
async def handle_movement_order(self, data: bytes, _: str) -> None: """ Handle move wheels packets. """ bus_message = BusMessage() bus_message.ParseFromString(data) # pylint: disable=no-member type_msg = bus_message.WhichOneof("message_content") if type_msg == "moveWheelAtSpeed": target_left = bus_message.moveWheelAtSpeed.left_tick_per_sec target_right = bus_message.moveWheelAtSpeed.right_tick_per_sec self.simulation_state.queue_speed_left.append(target_left) self.simulation_state.queue_speed_left.popleft() self.simulation_state.queue_speed_right.append(target_right) self.simulation_state.queue_speed_right.popleft() LOGGER.get().debug('simulation_router_received_wheel_speed_target') elif type_msg == "pidConfig": LOGGER.get().debug('simulation_router_received_pid_config') else: LOGGER.get().debug('simulation_router_received_unhandled_order')
async def _send_pid_configs(self, speed_left: PIDConstants, speed_right: PIDConstants, pos_left: PIDConstants, pos_right: PIDConstants) -> None: """ Sends last updated PID configurations. """ message = BusMessage(pidConfig=PIDConfigMsg( pid_speed_left=self._pid_constants_to_proto(speed_left), pid_speed_right=self._pid_constants_to_proto(speed_right), pid_position_left=self._pid_constants_to_proto(pos_left), pid_position_right=self._pid_constants_to_proto(pos_right), )) await self._send_message(message)
async def control_pump(self, adapter_index: int, pin: int, status: bool) -> None: """ Requests the activation or deactivation of a pump/valve on a given pin of a given board. @param board_id: in [0;NB_SERVO_BOARD]. @param pin: in [0;2], for each board. @param status: Boolean status to apply. True enables the given pump/valve. """ if pin < 0 or pin > 2: raise RuntimeError(f"Pump/Valve ID out of [0;2]:{pin}") msg = BusMessage(pumpAndValve=PumpAndValveMsg()) msg.pumpAndValve.id = pin msg.pumpAndValve.on = status await self._send_message(adapter_index, msg) LOGGER.get().debug('actuator_gw_control_pump', adapter_index=adapter_index, pin=pin, status=status)
async def dispatch_message(self, bus_message: BusMessage) -> None: """ Detect type of a BusMessage and call a controller. """ type_msg = bus_message.WhichOneof("message_content") if type_msg == "heartbeat": pass elif type_msg == "movementEnded": self.localization_controller.movement_done( bus_message.movementEnded.blocked) elif type_msg == "encoderPosition": self.localization_controller.update_odometry_position( bus_message.encoderPosition.left_tick, bus_message.encoderPosition.right_tick) elif type_msg == "laserSensor": await self.match_action.set_laser_distances() elif type_msg == "pressureSensor": await self.match_action.set_pressures() else: LOGGER.get().error("unhandled_protobuf_message", message_type=type_msg)