forked from Dan4ick/Lidar_Muxa
183 lines
7.6 KiB
Python
183 lines
7.6 KiB
Python
"""Пульт проигрывания записи: пауза, перемотка, шаг по кадру, скорость.
|
||
|
||
`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()
|