def get(self, name: str) -> Any:
"""
Get an instance of a class with its dependencies resolved.
"""
if name in self.instances:
return self.instances[name]
if name not in self.factories:
raise RuntimeError(
f"'{name}' never provided, add i.provide('{name}') in main.py")
cls = self.factories[name]
args_names = list(inspect.signature(cls).parameters.keys())
args = {}
for arg_name in args_names:
if arg_name in self.args[name]:
value = self.args[name][arg_name]
else:
value = self.get(arg_name)
if value is None:
raise RuntimeError(
f"{arg_name} not provided, needed by {name}")
args[arg_name] = value
LOGGER.get().info('injecting_dependency', name=name)
self.instances[name] = cls(**args)
return self.instances[name]
def _pid_constants_to_proto(
self, pid_constants: PIDConstants) -> PIDCoefficients:
"""
Converts a PIDConstants structure to a Protobuf PIDCoefficients sub message.
Applies a scaling factor to each value.
"""
scale_factor = self.configuration.pid_scale_factor
update_rate = self.configuration.motor_update_rate
max_value_on_motor_board = (2**32) - 1
if pid_constants.k_i * scale_factor / update_rate >= max_value_on_motor_board:
LOGGER.get().error('pid_to_proto_ki_too_large',
k_i=pid_constants.k_i)
raise RuntimeError(
"PID settings do not fit in motor board structures")
if pid_constants.k_d * scale_factor * update_rate >= max_value_on_motor_board:
LOGGER.get().error('pid_to_proto_kd_too_large',
k_d=pid_constants.k_d)
raise RuntimeError(
"PID settings do not fit in motor board structures")
return PIDCoefficients(
kp=pid_constants.k_p,
ki=pid_constants.k_i,
kd=pid_constants.k_d,
)
async def start_http_server(self, host: str,
port: int) -> Callable[[], Awaitable[None]]:
"""
Start the HTTP server. The HTTP server exposes a REST API to execute actions on the robot.
"""
LOGGER.get().info("http_server_start", port=port)
app = web.Application()
app.add_routes([
web.get('/action', self._http_get_actions),
web.post('/action', self._http_post_action),
])
async def on_prepare(_, response):
response.headers['Access-Control-Allow-Origin'] = '*'
app.on_response_prepare.append(on_prepare)
runner = web.AppRunner(app)
await runner.setup()
site = web.TCPSite(runner, host, port)
await site.start()
async def stop():
await runner.cleanup()
return stop
async def print_performance_metrics() -> None:
"""
Periodically log some performance metrics.
"""
loop = asyncio.get_event_loop()
while True:
# Measure elapsed time after a sleep.
# If the event loop is clogged, sleep will take more time to execute.
# For instance "sleep(1)" might take 1.5s to execute.
start = loop.time()
await asyncio.sleep(INTERVAL)
elapsed_time = loop.time() - start
delta = elapsed_time - INTERVAL
# Number of tasks scheduled on the event loop.
tasks = [t for t in Task.all_tasks(loop) if not t.done()]
active_tasks = len(tasks)
vmem = psutil.virtual_memory()
LOGGER.get().info(
"performance_stats",
event_loop_error=f"{delta/INTERVAL:.2%}",
event_loop_delta=f"{delta*1000:.3f}ms",
active_tasks=active_tasks,
cpu=f'{psutil.cpu_percent()/100:.2%}',
memory=f'{vmem.used / vmem.total:.2%}',
)
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 send(self, data: bytes) -> None:
hex_payload = binascii.hexlify(data)
LOGGER.get().debug('socket_can_adapter_send',
raw_payload=data,
hex_payload=hex_payload)
self.writer.write(b'<' + hex_payload + b'>\n')
await self.writer.drain()
async def rotate(self, angle: Radian) -> None:
"""
Make the robot rotate counter-clockwise and block until the movement is done.
"""
LOGGER.get().info('localization_controller_rotate', angle=angle)
self._state.movement_done_event.clear()
await self._state.movement_done_event.wait()
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 send(self, data: bytes) -> None:
"""Send a message."""
self.transport.sendto(data, self.address) # type: ignore
LOGGER.get().debug('isotp_socket_can_adapter_send',
payload=data,
name=self.adapter_name,
addresss=self.address)
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 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 move_forward(self, distance_mm: Millimeter) -> None:
"""
Make the robot move forward and block until the movement is done.
"""
LOGGER.get().info('localization_controller_move_forward',
distance=distance_mm)
self._state.movement_done_event.clear()
await self._state.movement_done_event.wait()
def datagram_received(self, data: bytes, addr: Tuple[str, int]) -> None:
for callback in self.callbacks:
asyncio.get_event_loop().create_task(
callback(data, self.adapter_name))
LOGGER.get().debug('isotp_socket_adapter_received',
payload=data,
name=self.adapter_name,
address=addr)
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 run(self) -> None:
"""
Run the strategy.
"""
for vec, reverse in PATH:
LOGGER.get().info("move robot", destination=vec)
await self.motion_controller.move_to(Vector2(vec.x, 2000 - vec.y),
reverse)
LOGGER.get().info("Strategy algorithm finished running") # lol
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 get_reader_writer(host: str,
port: int) -> Tuple[StreamReader, StreamWriter]:
"""
Try to open a TCP socket, retry if failed.
"""
while True:
try:
return await asyncio.open_connection(host, port)
except ConnectionRefusedError:
LOGGER.get().error("connection_refused", port=port, host=host)
async def _callback(self, websocket: websockets.WebSocketServerProtocol,
_: str) -> None:
"""
Function executed for each new incoming connection.
"""
LOGGER.get().info("new_debug_connection")
while websocket.state in (State.CONNECTING, State.OPEN):
data = self._simulation_probe.probe()
json_data = json.dumps(data, cls=RobotJSONEncoder)
await websocket.send(json_data)
await asyncio.sleep(1 / self._configuration.debug.refresh_rate)
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 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 run(self) -> None:
"""
Run the debug server.
"""
LOGGER.get().info("websocket_debug_serve")
async with websockets.serve(self._callback,
host=self._configuration.debug.host,
port=self._configuration.debug.port,
loop=self._event_loop,
process_request=process_request) as server:
await server.wait_closed()
async def run_websocket_server(self, host: str, port: int) -> None:
"""
Run the debug server.
"""
LOGGER.get().info("websocket_debug_serve", port=port)
async with websockets.serve(self._websocket_callback,
host=host,
port=port,
loop=self._event_loop,
process_request=process_request) as server:
await server.wait_closed()
def save_replay(self):
"""
Save the replay.
"""
dump = json.dumps(self.result, cls=RobotJSONEncoder)
LOGGER.get().info("saving_replay", size=len(dump))
replay_id = self.http_client.post_file(REPLAY_API_URL, dump)['id']
LOGGER.get().info("saved_replay", url=REPLAY_API_URL + replay_id)
self.web_browser_client.open(REPLAY_VIEWER_URL + '?replay=' +
REPLAY_API_URL + replay_id)
async def _run(self) -> None:
while True:
msg = await self.reader.readuntil(b'>')
msg = msg.strip(b'\n').strip(b'<>')
msg_str = msg.decode()
raw_payload = binascii.unhexlify(msg_str)
LOGGER.get().debug('socket_adapter_received',
raw_payload=raw_payload,
hex_payload=msg_str)
for handler in self.handlers:
await handler(raw_payload)
async def set_pid_speed(self, left_kp: float, left_ki: float,
left_kd: float, right_kp: float, right_ki: float,
right_kd: float) -> None:
"""
Sends the speed PID configurations for both wheels.
"""
pid_null = PIDConstants(0, 0, 0)
pid_speed_left = PIDConstants(left_kp, left_ki, left_kd)
pid_speed_right = PIDConstants(right_kp, right_ki, right_kd)
LOGGER.get().debug('motor_gateway_set_speed_pids',
pid_left=pid_speed_left,
pid_right=pid_speed_right)
await self._send_pid_configs(pid_speed_left, pid_speed_right, pid_null,
pid_null)
async def _run(self) -> None:
while True:
msg = await self.reader.readuntil(b'>')
msg = msg.strip(b'\n').strip(b'<>')
msg_str = msg.decode()
raw_payload = binascii.unhexlify(msg_str)
LOGGER.get().debug('socket_adapter_received',
raw_payload=raw_payload,
hex_payload=msg_str,
name=self.adapter_name)
for callback in self.callbacks:
await callback(raw_payload, self.adapter_name)
def update_odometry(self, tick_left: int, tick_right: int) -> None:
"""
Updates current position with new samples.
The first call will initialize the previous encoder positions used for deltas.
The position/angle will not be updated on this first call.
"""
self.position_left = tick_to_mm(
tick_left, self.configuration.encoder_ticks_per_revolution,
self.configuration.wheel_radius)
self.position_right = tick_to_mm(
tick_right, self.configuration.encoder_ticks_per_revolution,
self.configuration.wheel_radius)
self.probe.emit("encoder_left", self.position_left)
self.probe.emit("encoder_right", self.position_right)
if not self.initialized:
self.position_left_last = self.position_left
self.position_right_last = self.position_right
self.distance_init = (self.position_left + self.position_right) / 2
self.initialized = True
return
distance_old = self.distance_travelled
angle_old = self.angle
self.position, self.angle = self.odometry(
self.position_left - self.position_left_last,
self.position_right - self.position_right_last, self.position,
self.angle, self.configuration)
self.distance_travelled = ((self.position_left + self.position_right) /
2) - self.distance_init
LOGGER.get().debug('position_controller_update_odometry',
left_tick=tick_left,
right_tick=tick_right,
new_position=self.position,
new_angle=self.angle / (2 * math.pi) * 360)
self.position_left_last = self.position_left
self.position_right_last = self.position_right
self.speed = \
(self.distance_travelled - distance_old) * self.configuration.encoder_update_rate
self.angular_velocity = (
self.angle - angle_old) * self.configuration.encoder_update_rate
self.probe.emit("position", self.position)
self.probe.emit("angle", self.angle)
async def move_forward(self, distance_mm: Millimeter) -> None:
"""
Make the robot move forward and block until the movement is done.
"""
LOGGER.get().info('localization_controller_move_forward',
distance=distance_mm)
ticks_per_revolution = self.configuration.encoder_ticks_per_revolution
wheel_circumference = 2 * math.pi * self.configuration.wheel_radius
distance_revolution_count = distance_mm / wheel_circumference
distance_ticks = round(distance_revolution_count *
ticks_per_revolution)
self._state.movement_done_event.clear()
await self.motion_gateway.translate(distance_ticks)
await self._state.movement_done_event.wait()
async def run(self) -> None:
"""
Run the simulation.
"""
rate_encoder = self.configuration.encoder_update_rate
rate_tick = self.simulation_configuration.tickrate
while self.running:
current_tick = self.tick
last_left = self.state.queue_speed_left[-1]
last_right = self.state.queue_speed_right[-1]
self.state.queue_speed_left.append(
last_left *
numpy.random.normal(1, self.position_noise / 100.0))
self.state.queue_speed_left.popleft()
self.state.queue_speed_right.append(
last_right *
numpy.random.normal(1, self.position_noise / 100.0))
self.state.queue_speed_right.popleft()
# Send the encoder positions periodically.
interval = 1 / rate_encoder * 1000
if self.state.time - self.state.last_position_update >= interval:
self.state.left_tick += round(
mean(self.state.queue_speed_left) / rate_encoder)
self.state.right_tick += round(
mean(self.state.queue_speed_right) / rate_encoder)
self.state.last_position_update = self.state.time
await self.simulation_gateway.encoder_position(
self.state.left_tick, self.state.right_tick)
# Send the LIDAR positions periodically.
interval = 1 / self.simulation_configuration.lidar_position_rate * 1000
if self.state.time - self.state.last_lidar_update >= interval:
self.state.last_lidar_update = self.state.time
await self.simulation_gateway.push_lidar_readings()
self.tick = current_tick + 1
self.state.time = int(self.tick / rate_tick * 1000)
self.clock.fake_time = self.state.time / 1000
await asyncio.sleep(1 / rate_tick /
self.simulation_configuration.speed_factor)
LOGGER.get().info("simulation_runner_quit", time=self.state.time)
