Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Verzögerung eingebaut

Browse Source
This commit is contained in:
Eskimue
2026-05-21 20:12:02 +02:00
parent 1c139887fd
commit f53d51acb6
9 changed files with 346 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ExoMy_Software-master/docker/entrypoint.sh
+5 -2
View File
@@ -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
ExoMy_Software-master/gui/admin.html
+62
View File
@@ -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>
ExoMy_Software-master/gui/index.html
+2 -2
View File
@@ -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");
});
ExoMy_Software-master/gui/style.css
+36
View File
@@ -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;
}
ExoMy_Software-master/scripts/exomy-video-delay.service
+14
View File
@@ -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
ExoMy_Software-master/scripts/exomy_admin_api.py
+49
View File
@@ -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'))
ExoMy_Software-master/scripts/video_delay_proxy.py
+140
View File
@@ -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()
ExoMy_Software-master/src/delay_node.py
+37
View File
@@ -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()
ExoMy_Software-master/src/joystick_parser_node.py
+1 -1
View File
@@ -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()