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)
