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)