"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость. `ros2 bag play`, запущенный из launch (`bag:=...`), клавиатуры не слышит — у него нет терминала. Пульт управляет им через сервисы rosbag2 из второго терминала того же контейнера: docker exec -it flyguard_demo flyguard-keys /data/<запись> пробел пауза / продолжить ← → на 3 секунды назад / вперёд . один кадр вперёд (на паузе) ↑ ↓ быстрее / медленнее 0 в начало записи q выйти (запись играет дальше) Текущее время записи берётся из /clock — launch запускает проигрыватель с `--clock`. Путь к записи нужен только для клавиши «0»: из metadata.yaml читается время её начала. Узел детектора после перемотки сам сбрасывает треки и одометрию (скачок времени, `pipeline.FlyGuard.process`). """ from __future__ import annotations import re import select import sys import termios import tty from pathlib import Path import rclpy from builtin_interfaces.msg import Time from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data from rosbag2_interfaces.srv import GetRate, IsPaused, PlayNext, Seek, SetRate, TogglePaused from rosgraph_msgs.msg import Clock STEP_S = 3.0 RATES = [0.1, 0.25, 0.5, 1.0, 2.0, 4.0] PLAYER = "/rosbag2_player/" KEYS = """ пробел пауза / продолжить ← → на 3 секунды назад / вперёд . один кадр вперёд (на паузе) ↑ ↓ быстрее / медленнее 0 в начало записи q выйти (запись играет дальше) """ def bag_span(path: str | None) -> tuple[float | None, float | None]: """Начало и длительность записи из metadata.yaml, секунды.""" if not path: return None, None meta = Path(path) / "metadata.yaml" if not meta.exists(): return None, None txt = meta.read_text(encoding="utf-8") s = re.search(r"starting_time:\s*\n\s*nanoseconds_since_epoch:\s*(\d+)", txt) d = re.search(r"duration:\s*\n\s*nanoseconds:\s*(\d+)", txt) return (int(s.group(1)) * 1e-9 if s else None, int(d.group(1)) * 1e-9 if d else None) class PlayerKeys(Node): def __init__(self, start: float | None, duration: float | None): super().__init__("flyguard_player_keys") self.now: float | None = None self.start = start self.duration = duration self.state, self.rate = "идёт", 1.0 # /clock проигрыватель шлёт без подтверждений (best effort): с надёжной # подпиской QoS не сходится, и время не приходит вовсе self.create_subscription(Clock, "/clock", self._on_clock, qos_profile_sensor_data) self.cli = {name: self.create_client(srv, PLAYER + name) for name, srv in ( ("toggle_paused", TogglePaused), ("is_paused", IsPaused), ("seek", Seek), ("set_rate", SetRate), ("get_rate", GetRate), ("play_next", PlayNext))} def _on_clock(self, msg: Clock) -> None: self.now = msg.clock.sec + msg.clock.nanosec * 1e-9 if self.start is None: self.start = self.now def call(self, name: str, req=None): cli = self.cli[name] if not cli.wait_for_service(timeout_sec=2.0): self.say("проигрыватель не отвечает — запись закончилась или запущена без launch?") return None fut = cli.call_async(req if req is not None else cli.srv_type.Request()) rclpy.spin_until_future_complete(self, fut, timeout_sec=3.0) return fut.result() def seek(self, t: float) -> None: # за конец записи проигрыватель просто закончит её — не пускаем if self.start is not None: t = max(t, self.start) if self.duration: t = min(t, self.start + self.duration - 0.5) req = Seek.Request() sec = int(t) req.time = Time(sec=sec, nanosec=int((t - sec) * 1e9)) self.call("seek", req) def set_rate(self, up: bool) -> None: got = self.call("get_rate") cur = got.rate if got else 1.0 if up: new = next((r for r in RATES if r > cur + 1e-6), RATES[-1]) else: new = next((r for r in reversed(RATES) if r < cur - 1e-6), RATES[0]) req = SetRate.Request() req.rate = float(new) self.call("set_rate", req) def status(self, ask: bool = True) -> str: """Строка состояния; ask=False — без запросов к проигрывателю.""" if ask: paused = self.call("is_paused") rate = self.call("get_rate") self.state = "ПАУЗА" if paused and paused.paused else "идёт" self.rate = rate.rate if rate else 1.0 pos = (f"{self.now - self.start:6.1f} с" if self.now is not None and self.start else " ? с") return f"{pos} {self.state:6s} скорость ×{self.rate:g}" @staticmethod def say(text: str) -> None: sys.stdout.write("\r\033[K" + text) sys.stdout.flush() def read_key(fd: int) -> str: ch = sys.stdin.read(1) if ch != "\x1b": return ch if select.select([fd], [], [], 0.05)[0]: seq = sys.stdin.read(2) return {"[A": "up", "[B": "down", "[C": "right", "[D": "left"}.get(seq, "") return "esc" def main(argv=None) -> None: args = [a for a in (argv or sys.argv[1:]) if not a.startswith("--")] rclpy.init() node = PlayerKeys(*bag_span(args[0] if args else None)) print(KEYS) fd = sys.stdin.fileno() old = termios.tcgetattr(fd) try: tty.setcbreak(fd) node.say(node.status()) while rclpy.ok(): if not select.select([fd], [], [], 0.2)[0]: rclpy.spin_once(node, timeout_sec=0.0) node.say(node.status(ask=False)) # бегущее время записи continue key = read_key(fd) rclpy.spin_once(node, timeout_sec=0.05) if key in ("q", "й", "esc"): break if key == " ": node.call("toggle_paused") elif key in ("left", "right") and node.now is not None: node.seek(node.now + (STEP_S if key == "right" else -STEP_S)) elif key in ("left", "right"): node.say("ещё нет времени записи (/clock) — подождите секунду") continue elif key in (".", "ю"): node.call("play_next") elif key in ("up", "down"): node.set_rate(key == "up") elif key == "0" and node.start is not None: node.seek(node.start) rclpy.spin_once(node, timeout_sec=0.1) node.say(node.status()) except KeyboardInterrupt: pass finally: termios.tcsetattr(fd, termios.TCSADRAIN, old) print() node.destroy_node() rclpy.try_shutdown() if __name__ == "__main__": main()