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 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_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_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_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 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 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 translate_message(self, msg: bytes) -> None: """ Convert bytes to BusMessage. """ bus_message = BusMessage() bus_message.ParseFromString(msg) LOGGER.get().debug('msg_can', msg=bus_message) await self.dispatch_message(bus_message)