diff --git a/ExoMy_Software-master/docker/entrypoint.sh b/ExoMy_Software-master/docker/entrypoint.sh index 21a7fbc..e5a30a7 100644 --- a/ExoMy_Software-master/docker/entrypoint.sh +++ b/ExoMy_Software-master/docker/entrypoint.sh @@ -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 diff --git a/ExoMy_Software-master/gui/admin.html b/ExoMy_Software-master/gui/admin.html index 14a5733..3b02ee2 100644 --- a/ExoMy_Software-master/gui/admin.html +++ b/ExoMy_Software-master/gui/admin.html @@ -72,6 +72,34 @@ +
+
+

Latenz-Simulation

+
+
+
+ Signallaufzeit +

Verzögert Steuerbefehle (Controller + Web-GUI) und Kamerabild gleichzeitig.

+
+
+ + +
+
+
+

Systemstatus

@@ -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); }); diff --git a/ExoMy_Software-master/gui/index.html b/ExoMy_Software-master/gui/index.html index 40b2144..48617d1 100644 --- a/ExoMy_Software-master/gui/index.html +++ b/ExoMy_Software-master/gui/index.html @@ -629,7 +629,7 @@ header {
HOST: --
- STREAM: 8081 + STREAM: 8083
@@ -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"); }); diff --git a/ExoMy_Software-master/gui/style.css b/ExoMy_Software-master/gui/style.css index 969a9de..8d1fc61 100644 --- a/ExoMy_Software-master/gui/style.css +++ b/ExoMy_Software-master/gui/style.css @@ -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; } diff --git a/ExoMy_Software-master/scripts/exomy-video-delay.service b/ExoMy_Software-master/scripts/exomy-video-delay.service new file mode 100644 index 0000000..7979819 --- /dev/null +++ b/ExoMy_Software-master/scripts/exomy-video-delay.service @@ -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 diff --git a/ExoMy_Software-master/scripts/exomy_admin_api.py b/ExoMy_Software-master/scripts/exomy_admin_api.py index bc0a925..bc04b27 100644 --- a/ExoMy_Software-master/scripts/exomy_admin_api.py +++ b/ExoMy_Software-master/scripts/exomy_admin_api.py @@ -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')) diff --git a/ExoMy_Software-master/scripts/video_delay_proxy.py b/ExoMy_Software-master/scripts/video_delay_proxy.py new file mode 100644 index 0000000..386a511 --- /dev/null +++ b/ExoMy_Software-master/scripts/video_delay_proxy.py @@ -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() diff --git a/ExoMy_Software-master/src/delay_node.py b/ExoMy_Software-master/src/delay_node.py new file mode 100644 index 0000000..f432ee8 --- /dev/null +++ b/ExoMy_Software-master/src/delay_node.py @@ -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() diff --git a/ExoMy_Software-master/src/joystick_parser_node.py b/ExoMy_Software-master/src/joystick_parser_node.py index ead333b..dddfe64 100644 --- a/ExoMy_Software-master/src/joystick_parser_node.py +++ b/ExoMy_Software-master/src/joystick_parser_node.py @@ -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()