Verzögerung eingebaut

2026-05-21 20:12:02 +02:00
parent 1c139887fd
commit f53d51acb6
9 changed files with 346 additions and 5 deletions
@@ -2,7 +2,7 @@
 set -eo pipefail

 cleanup() {
-	for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID; do
+	for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID; do
 		if [[ -n "${!pid_var:-}" ]]; then
 			kill "${!pid_var}" 2>/dev/null || true
 		fi
@@ -34,11 +34,14 @@ then

 	rosparam load /root/exomy_ws/src/exomy/config/exomy.yaml
 	rosparam set /controller logitech-F710
+	rosparam set /delay_seconds 0.0

 	/opt/ros/melodic/lib/rosbridge_server/rosbridge_websocket > /tmp/rosbridge.log 2>&1 &
 	ROSBRIDGE_PID=$!
 	/opt/ros/melodic/lib/rosapi/rosapi_node > /tmp/rosapi.log 2>&1 &
 	ROSAPI_PID=$!
+	python /root/exomy_ws/src/exomy/src/delay_node.py > /tmp/delay_node.log 2>&1 &
+	DELAY_PID=$!
 	python /root/exomy_ws/src/exomy/src/f710_joy_node.py > /tmp/joy_node.log 2>&1 &
 	JOY_PID=$!
 	python /root/exomy_ws/src/exomy/src/robot_node.py > /tmp/robot_node.log 2>&1 &
@@ -48,7 +51,7 @@ then
 	python /root/exomy_ws/src/exomy/src/joystick_parser_node.py > /tmp/joystick_parser.log 2>&1 &
 	JOYSTICK_PID=$!

-	wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID"
+	wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID"
 	exit 1
 elif [[ $1 == "devel" ]]
 then
@@ -72,6 +72,34 @@
            </div>
          </div>

+          <div class="admin_section">
+            <div class="admin_section_header">
+              <p class="eyebrow">Latenz-Simulation</p>
+            </div>
+            <div class="admin_quick_link_card">
+              <div>
+                <strong>Signallaufzeit</strong>
+                <p>Verzögert Steuerbefehle (Controller + Web-GUI) und Kamerabild gleichzeitig.</p>
+              </div>
+              <div class="delay_control">
+                <select id="delay_select" class="delay_select" onchange="setDelay(parseFloat(this.value))">
+                  <option value="0">0 s – kein Delay</option>
+                  <option value="1">1 s</option>
+                  <option value="2">2 s</option>
+                  <option value="3">3 s</option>
+                  <option value="4">4 s</option>
+                  <option value="5">5 s</option>
+                  <option value="6">6 s</option>
+                  <option value="7">7 s</option>
+                  <option value="8">8 s</option>
+                  <option value="9">9 s</option>
+                  <option value="10">10 s</option>
+                </select>
+                <span id="delay_status" class="delay_status_text"></span>
+              </div>
+            </div>
+          </div>
+
          <div class="admin_section">
            <div class="admin_section_header">
              <p class="eyebrow">Systemstatus</p>
@@ -246,9 +274,43 @@
      }
    }

+    function updateDelayButtons(seconds) {
+      var sel = document.getElementById('delay_select');
+      if (sel) sel.value = String(seconds);
+    }
+
+    async function fetchDelay() {
+      try {
+        var response = await fetch(adminApiBase + '/api/delay');
+        if (!response.ok) return;
+        var data = await response.json();
+        updateDelayButtons(data.delay_seconds);
+      } catch (e) {}
+    }
+
+    async function setDelay(seconds) {
+      var statusEl = document.getElementById('delay_status');
+      if (statusEl) statusEl.textContent = 'Setze…';
+      try {
+        var response = await fetch(adminApiBase + '/api/delay', {
+          method: 'POST',
+          headers: {'Content-Type': 'application/json'},
+          body: JSON.stringify({delay_seconds: seconds})
+        });
+        var data = await response.json();
+        updateDelayButtons(seconds);
+        if (statusEl) statusEl.textContent = data.status || 'Gesetzt';
+        window.setTimeout(function() { if (statusEl) statusEl.textContent = ''; }, 2000);
+      } catch (e) {
+        if (statusEl) statusEl.textContent = 'Fehler';
+      }
+    }
+
    window.addEventListener('load', function () {
      fetchStatus();
+      fetchDelay();
      window.setInterval(fetchStatus, 10000);
+      window.setInterval(fetchDelay, 5000);
    });
  </script>
 </body>
@@ -629,7 +629,7 @@ header {
        </div>
        <div class="c-br">
          HOST: <span id="cam-host">--</span><br>
-          STREAM: 8081
+          STREAM: 8083
        </div>
        <div class="c-xhair">
          <svg viewBox="-90 -90 180 180" xmlns="http://www.w3.org/2000/svg" width="180" height="180">
@@ -1152,7 +1152,7 @@ window.addEventListener("load", function () {
  statusTimer = window.setInterval(fetchSystemStatus, 5000);

  var video = document.getElementById("video_output");
-  video.src = "//" + hostUrl + ":8081/stream.mjpg";
+  video.src = "//" + hostUrl + ":8083/stream.mjpg";
  video.addEventListener("load", function () {
    setText("cam-state", "Online");
  });
@@ -584,6 +584,42 @@ h2 {
    0 8px 18px rgba(109, 126, 132, 0.08);
 }

+.delay_control {
+  display: flex;
+  flex-direction: column;
+  align-items: flex-end;
+  gap: 0.45rem;
+}
+
+.delay_select {
+  appearance: none;
+  -webkit-appearance: none;
+  width: 100%;
+  padding: 0.65rem 2.2rem 0.65rem 0.85rem;
+  border-radius: var(--radius-sm);
+  border: 1px solid rgba(116, 143, 152, 0.35);
+  background: #fff url("data:image/svg+xml,%3Csvg xmlns='http://www.w3.org/2000/svg' width='12' height='8' viewBox='0 0 12 8'%3E%3Cpath d='M1 1l5 5 5-5' stroke='%23617781' stroke-width='1.6' fill='none' stroke-linecap='round'/%3E%3C/svg%3E") right 0.75rem center no-repeat;
+  font-family: Consolas, "Courier New", monospace;
+  font-size: 0.95rem;
+  font-weight: 700;
+  color: #1a2830;
+  cursor: pointer;
+  transition: border-color 0.15s, box-shadow 0.15s;
+}
+
+.delay_select:focus {
+  outline: none;
+  border-color: var(--pri);
+  box-shadow: 0 0 0 3px rgba(0, 136, 255, 0.15);
+}
+
+.delay_status_text {
+  font-size: 0.78rem;
+  color: var(--muted);
+  min-height: 1em;
+  text-align: right;
+}
+
 .admin-page .admin_quick_link_card p {
  color: #617781;
 }
@@ -0,0 +1,14 @@
+[Unit]
+Description=ExoMy Video Delay Proxy
+After=network.target exomy-camera-stream.service
+Requires=exomy-camera-stream.service
+
+[Service]
+Type=simple
+User=pi
+ExecStart=/usr/bin/python3 /home/pi/ExoMy_Software/scripts/video_delay_proxy.py
+Restart=always
+RestartSec=3
+
+[Install]
+WantedBy=multi-user.target
@@ -10,6 +10,7 @@ import time

 PORT = 8082
 EXOMY_CONTAINER = 'exomy_autostart'
+DELAY_FILE = '/tmp/exomy_delay.txt'
 MOTOR_TEST_LOCK = threading.Lock()


@@ -220,6 +221,33 @@ def collect_status():
    }


+def get_delay():
+    try:
+        with open(DELAY_FILE) as f:
+            return max(0.0, min(10.0, float(f.read().strip())))
+    except Exception:
+        return 0.0
+
+
+def set_delay(seconds):
+    seconds = max(0.0, min(10.0, float(seconds)))
+    data = str(seconds).encode()
+    try:
+        fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_TRUNC)
+    except FileNotFoundError:
+        fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o666)
+    try:
+        os.write(fd, data)
+    finally:
+        os.close(fd)
+    subprocess.run(
+        ['docker', 'exec', EXOMY_CONTAINER, 'bash', '-c',
+         f'source /opt/ros/melodic/setup.bash && rosparam set /delay_seconds {seconds}'],
+        capture_output=True, check=False
+    )
+    return seconds
+
+
 def restart_camera_service():
    subprocess.Popen(['systemctl', 'restart', 'exomy-camera-stream.service'])

@@ -449,6 +477,9 @@ class Handler(http.server.BaseHTTPRequestHandler):
        self.end_headers()

    def do_GET(self):
+        if self.path == '/api/delay':
+            self.write_json(200, {'delay_seconds': get_delay()})
+            return
        if self.path == '/api/status':
            self.write_json(200, collect_status())
            return
@@ -469,6 +500,24 @@ class Handler(http.server.BaseHTTPRequestHandler):
        self.write_json(404, {'status': 'Unbekannt'})

    def do_POST(self):
+        if self.path == '/api/delay':
+            try:
+                content_length = int(self.headers.get('Content-Length', '0'))
+            except ValueError:
+                content_length = 0
+            raw_body = self.rfile.read(content_length) if content_length > 0 else b'{}'
+            try:
+                payload = json.loads(raw_body.decode('utf-8'))
+            except (UnicodeDecodeError, json.JSONDecodeError):
+                self.write_json(400, {'status': 'Ungültige Anfrage'})
+                return
+            try:
+                seconds = set_delay(payload.get('delay_seconds', 0))
+                self.write_json(200, {'status': 'Delay gesetzt', 'delay_seconds': seconds})
+            except Exception as exc:
+                self.write_json(500, {'status': str(exc)})
+            return
+
        if self.path == '/api/motor-test/drive-neutral/preview':
            try:
                content_length = int(self.headers.get('Content-Length', '0'))
@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+"""
+MJPEG Video Delay Proxy
+Puffert Frames vom Kamerastream (Port 8081) und liefert sie
+mit einstellbarer Verzögerung auf Port 8083.
+Delay wird aus /tmp/exomy_delay.txt gelesen (Sekunden als float).
+"""
+import collections
+import http.server
+import os
+import socketserver
+import threading
+import time
+import urllib.request
+
+DELAY_FILE = '/tmp/exomy_delay.txt'
+SOURCE_URL = 'http://localhost:8081/stream.mjpg'
+PORT = 8083
+BOUNDARY = b'--frame'
+
+# Deque: (timestamp_float, jpeg_bytes)
+frame_buffer = collections.deque()
+buffer_lock = threading.Lock()
+latest_frame = None
+latest_lock = threading.Lock()
+
+
+def read_delay():
+    try:
+        with open(DELAY_FILE) as f:
+            return max(0.0, float(f.read().strip()))
+    except Exception:
+        return 0.0
+
+
+def camera_reader():
+    global latest_frame
+    while True:
+        try:
+            req = urllib.request.urlopen(SOURCE_URL, timeout=5)
+            buf = bytearray()
+            while True:
+                chunk = req.read(4096)
+                if not chunk:
+                    break
+                buf.extend(chunk)
+                while True:
+                    start = buf.find(b'\xff\xd8')
+                    if start == -1:
+                        if len(buf) > 1024 * 1024:
+                            del buf[:-2]
+                        break
+                    end = buf.find(b'\xff\xd9', start + 2)
+                    if end == -1:
+                        if start > 0:
+                            del buf[:start]
+                        break
+                    jpeg = bytes(buf[start:end + 2])
+                    del buf[:end + 2]
+                    ts = time.monotonic()
+                    with buffer_lock:
+                        frame_buffer.append((ts, jpeg))
+                        # Puffer auf 12 Sekunden begrenzen
+                        cutoff = ts - 12.0
+                        while frame_buffer and frame_buffer[0][0] < cutoff:
+                            frame_buffer.popleft()
+                    with latest_lock:
+                        latest_frame = jpeg
+        except Exception:
+            time.sleep(1)
+
+
+def get_delayed_frame():
+    delay = read_delay()
+    if delay <= 0:
+        with latest_lock:
+            return latest_frame
+    target_ts = time.monotonic() - delay
+    with buffer_lock:
+        if not frame_buffer:
+            return None
+        best = frame_buffer[0][1]
+        for ts, jpeg in frame_buffer:
+            if ts <= target_ts:
+                best = jpeg
+            else:
+                break
+        return best
+
+
+class ReusableTCPServer(socketserver.ThreadingTCPServer):
+    allow_reuse_address = True
+
+
+class Handler(http.server.BaseHTTPRequestHandler):
+    def do_GET(self):
+        if self.path != '/stream.mjpg':
+            self.send_error(404)
+            return
+        self.send_response(200)
+        self.send_header('Age', '0')
+        self.send_header('Cache-Control', 'no-cache, private')
+        self.send_header('Pragma', 'no-cache')
+        self.send_header('Content-Type', 'multipart/x-mixed-replace; boundary=frame')
+        self.end_headers()
+        fps = 10
+        while True:
+            frame = get_delayed_frame()
+            if frame is None:
+                time.sleep(0.1)
+                continue
+            try:
+                self.wfile.write(BOUNDARY + b'\r\n')
+                self.wfile.write(b'Content-Type: image/jpeg\r\n')
+                self.wfile.write(f'Content-Length: {len(frame)}\r\n\r\n'.encode())
+                self.wfile.write(frame)
+                self.wfile.write(b'\r\n')
+                self.wfile.flush()
+                time.sleep(1.0 / fps)
+            except (BrokenPipeError, ConnectionResetError):
+                break
+
+    def log_message(self, fmt, *args):
+        return
+
+
+if __name__ == '__main__':
+    # Delay-Datei initialisieren (world-writable damit Admin-API schreiben kann)
+    try:
+        fd = os.open(DELAY_FILE, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o666)
+        os.write(fd, b'0.0')
+        os.close(fd)
+    except Exception:
+        pass
+
+    threading.Thread(target=camera_reader, daemon=True).start()
+    with ReusableTCPServer(('0.0.0.0', PORT), Handler) as server:
+        server.daemon_threads = True
+        print(f'Video-Delay-Proxy läuft auf Port {PORT}')
+        server.serve_forever()
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import collections
+import rospy
+from sensor_msgs.msg import Joy
+
+queue = collections.deque()
+pub = None
+
+
+def callback(msg):
+    delay = rospy.get_param('/delay_seconds', 0.0)
+    if delay <= 0:
+        pub.publish(msg)
+        return
+    queue.append((rospy.Time.now().to_sec(), msg))
+
+
+def spin():
+    rate = rospy.Rate(50)
+    while not rospy.is_shutdown():
+        delay = rospy.get_param('/delay_seconds', 0.0)
+        now = rospy.Time.now().to_sec()
+        if delay <= 0:
+            while queue:
+                pub.publish(queue.popleft()[1])
+        else:
+            while queue and (now - queue[0][0]) >= delay:
+                pub.publish(queue.popleft()[1])
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('delay_node')
+    rospy.loginfo('delay_node gestartet')
+    pub = rospy.Publisher('/joy_delayed', Joy, queue_size=50)
+    rospy.Subscriber('/joy', Joy, callback, queue_size=50)
+    spin()
@@ -193,7 +193,7 @@ if __name__ == '__main__':
    rospy.init_node('joystick_parser_node')
    rospy.loginfo('joystick_parser_node started')

-    sub = rospy.Subscriber("/joy", Joy, callback, queue_size=1)
+    sub = rospy.Subscriber("/joy_delayed", Joy, callback, queue_size=1)
    pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)

    rospy.spin()