Compare commits

...
10 Commits
Author SHA1 Message Date
EskimueandClaude Sonnet 4.6 9218b95e6e Dreh-Speed Dropdown im Admin, Kalibrierungsversuch rückgängig gemacht
- Auto-Rotate Speed wählbar: 20-50% in 5er-Schritten (Standard 20%)
- BNO085 Magnetometer-Kalibrierung: calibration_status liefert immer 0,
  da der Sensor 3D-Bewegung benötigt die am Boden nicht möglich ist.
  Kalibrierungs-Feature vollständig entfernt.
- Empfehlung: Nord-Offset nach jedem Neustart neu setzen.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-30 08:52:10 +02:00
EskimueandClaude Sonnet 4.6 0c4cafd12b Auto-Rotate Dokumentation in README, F710-Blockierung bestätigt
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-30 07:18:12 +02:00
EskimueandClaude Sonnet 4.6 13af8052da Auto-Rotate Statusanzeige stabil mit fester Breite und Nachkommastelle
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-29 10:51:48 +02:00
EskimueandClaude Sonnet 4.6 ff58356c23 Auto-Rotate blockiert Joystick-Parser während der Drehung
rosparam /auto_rotate_active pausiert joystick_parser_node komplett.
Modus-Wiederherstellung erst nach Erreichen der Zielposition.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-29 10:37:44 +02:00
EskimueandClaude Sonnet 4.6 01e7d47b4e Aktiver Fahrmodus und Eingabe rot hervorgehoben mit sanftem Pulse
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-29 10:32:39 +02:00
EskimueandClaude Sonnet 4.6 93772ec3b7 HUD-Farbe konfigurierbar, Neigungskalibrierung, Auto-Rotate verbessert
- HUD-Farbe per Farbwähler im Admin einstellbar (localStorage)
- Schwarze Kontur auf HUD-Elementen via CSS drop-shadow und canvas shadowBlur
- Himmelsrichtungen auf Deutsch (O statt E, etc.)
- Roll/Pitch-Neigung kann im Admin genullt werden (imu_tilt_calibration.json)
- Neigungsoffset wird im Kamera-Overlay berücksichtigt
- Auto-Rotate läuft jetzt nativ als rospy-Script im Docker-Container
- Auto-Rotate erkennt aktuellen Locomotion-Mode und stellt ihn danach wieder her
- Auto-Rotate pausiert Web-GUI-Joystick via localStorage-Flag
- Feste 20%-Geschwindigkeit für Auto-Rotate (vel=4, Expo-äquivalent)
- Vignette entfernt

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-05-29 10:26:11 +02:00
Eskimue 4c24558516 Turn on Point kann jetzt schon im Admin aufgerufen werden um den Rover in eine Richtug zu drehen 2026-05-29 09:36:53 +02:00
Eskimue 5e39a41f49 GUI 2026-05-29 09:20:28 +02:00
Eskimue b4ea4799a9 IMU kann genullt werden 2026-05-29 08:52:41 +02:00
Eskimue 5bfd5797fb IMU X und Y getauscht 2026-05-28 20:32:45 +02:00
9 changed files with 779 additions and 69 deletions
+62
View File
@@ -162,6 +162,14 @@ docker exec -it exomy_autostart bash
- Grund: Der restliche ROS-Stack im Container nutzt noch überwiegend Python 2, der BNO085-Treiber benötigt aber Python 3.
- Der Service `exomy-imu-ros.service` liest den Sensor und publiziert über `rosbridge` nach ROS.
### IMU-Einbaulage und Achsen-Remapping
Der BNO085 ist um 90° um die Z-Achse verdreht eingebaut. Dadurch wären die X- und Y-Achsen des Sensors gegenüber dem Rover-Koordinatensystem vertauscht — Roll und Pitch kämen vertauscht an.
`imu_node.py` korrigiert das direkt beim Publizieren: X- und Y-Komponenten werden für Quaternion, Gyro, Beschleunigung und Magnetfeld getauscht. Alle anderen Teile des Systems (GUI, Admin-API) sehen bereits korrekte Daten und müssen nichts kompensieren.
Wenn der Sensor jemals neu ausgerichtet eingebaut wird, muss das Remapping in `publish_measurements()` in `imu_node.py` entsprechend angepasst oder entfernt werden.
### IMU-Topics
| Topic | Typ | Inhalt |
@@ -278,6 +286,60 @@ math.atan((self.wheel_rx + self.wheel_fx) / self.wheel_ry))
---
## Auto-Rotate (automatisches Drehen zu einem Ziel-Heading)
Die Admin-Seite (`http://<IP>:8000/admin.html`) enthält im IMU-Bereich eine Funktion, mit der der Rover automatisch zu einem eingegebenen Ziel-Heading dreht.
### Bedienung
1. Zielwinkel eingeben (0–359°, wobei 0 = Nord, 90 = Ost, 180 = Süd, 270 = West — **nach Nord-Kalibrierung**)
2. „Zu Heading drehen" drücken
3. Der Rover dreht auf der Stelle und stoppt automatisch bei ±1° Genauigkeit
4. „Drehung abbrechen" bricht den Vorgang vorzeitig ab
### Technischer Ablauf
```
Admin-API
→ startet auto_rotate_ros.py via docker exec im Container
→ setzt rosparam /auto_rotate_active = True
→ joystick_parser_node ignoriert ALLE Eingaben (Web-GUI, D-Pad, F710)
→ auto_rotate_ros.py dreht mit Point-Turn-Modus bei fester Geschwindigkeit (~20 %)
→ bei Ziel erreicht (oder Abbruch):
→ rosparam /auto_rotate_active = False ← erst jetzt wird Steuerung freigegeben
→ letzter Fahrmodus (vor der Drehung) wird wiederhergestellt
```
### Wichtige Hinweise
- **Geschwindigkeit:** Fest auf ~20 % (entspricht `vel=4` durch die Expo-Kurve), unabhängig vom eingestellten Speed-Limit in der Admin-Seite.
- **Nord-Kalibrierung muss gesetzt sein:** Der Zielwinkel bezieht sich auf den kalibrierten Norden. Ohne Kalibrierung dreht der Rover zum magnetischen Roh-Heading.
- **Kein manuelles Eingreifen während der Drehung:** Joystick, D-Pad und F710 sind über `rosparam /auto_rotate_active` vollständig blockiert. Erst nach Abschluss oder Abbruch wird die Steuerung freigegeben.
- **Fahrmodus:** Der Rover wechselt intern in den Point-Turn-Modus und kehrt danach automatisch in den zuvor aktiven Modus zurück (Ackermann, Point Turn oder Crabbing).
- **Neigungskalibrierung:** Wenn die IMU-Neigung genullt wurde, gilt das nur für die Anzeige — die Rotation selbst nutzt nur den Yaw-Winkel.
### Dateien
| Datei | Beschreibung |
|---|---|
| `scripts/auto_rotate_ros.py` | Läuft im Docker-Container, nativer rospy-Node |
| `scripts/imu_rotate.py` | Steuerung des docker-exec-Prozesses, Status-Tracking |
### Deploy nach Änderungen
```bash
# auto_rotate_ros.py oder joystick_parser_node.py geändert:
scp scripts/auto_rotate_ros.py pi@192.168.1.9:/home/pi/ExoMy_Software/scripts/
scp src/joystick_parser_node.py pi@192.168.1.9:/home/pi/ExoMy_Software/src/
ssh pi@192.168.1.9 "sudo docker restart exomy_autostart"
# imu_rotate.py oder exomy_admin_api.py geändert:
scp scripts/imu_rotate.py scripts/exomy_admin_api.py pi@192.168.1.9:/home/pi/ExoMy_Software/scripts/
ssh pi@192.168.1.9 "sudo systemctl restart exomy-admin-api.service"
```
---
## Fahrmodi
| Modus | Taste (Controller) | Taste (Web-GUI) |
+201
View File
@@ -153,8 +153,65 @@
<span class="sr-key">Nord-Offset</span>
<strong id="imu_heading_offset" class="sr-val">-</strong>
</div>
<div class="sr">
<span class="sr-led led-ok"></span>
<span class="sr-key">Roll korr.</span>
<strong id="imu_roll_corrected" class="sr-val">-</strong>
</div>
<div class="sr">
<span class="sr-led led-ok"></span>
<span class="sr-key">Pitch korr.</span>
<strong id="imu_pitch_corrected" class="sr-val">-</strong>
</div>
<div class="sr">
<span class="sr-led led-off"></span>
<span class="sr-key">Neig.-Offset</span>
<strong id="imu_tilt_offset" class="sr-val">-</strong>
</div>
<div class="act-grid">
<button class="act-btn subtle full" onclick="setImuNorth()">Aktuelle Richtung als Norden setzen</button>
<button class="act-btn subtle full" onclick="setImuLevel()">Aktuelle Neigung als Null setzen</button>
</div>
<div class="sr">
<span class="sr-led led-off"></span>
<span class="sr-key">HUD-Farbe</span>
<span class="sr-val" style="display:flex;align-items:center;gap:6px;">
<input type="color" id="hud_color_picker" value="#00ff91" style="width:32px;height:22px;border:none;background:none;cursor:pointer;padding:0;" oninput="applyHudColorFromAdmin(this.value)" onchange="applyHudColorFromAdmin(this.value)">
<button class="act-btn subtle" style="padding:2px 8px;font-size:11px;" onclick="resetHudColor()">Reset</button>
</span>
</div>
<div class="sr">
<span class="sr-led led-off"></span>
<span class="sr-key">Ziel-Heading</span>
<span class="sr-val" style="display:flex;align-items:center;gap:6px;">
<input type="number" id="rotate_target" min="0" max="359" step="1" value="0"
style="width:60px;background:#111;color:#eee;border:1px solid #444;padding:2px 4px;font-size:12px;border-radius:3px;">
<span style="font-size:11px;color:#888;">° (0=N, 90=O, 180=S, 270=W)</span>
</span>
</div>
<div class="sr">
<span class="sr-led led-off"></span>
<span class="sr-key">Dreh-Speed</span>
<span class="sr-val">
<select id="rotate_speed" class="ctrl-select" style="font-size:11px;padding:2px 4px;">
<option value="20" selected>20 % (Standard)</option>
<option value="25">25 %</option>
<option value="30">30 %</option>
<option value="35">35 %</option>
<option value="40">40 %</option>
<option value="45">45 %</option>
<option value="50">50 %</option>
</select>
</span>
</div>
<div class="act-grid">
<button class="act-btn subtle full" id="rotate_start_btn" onclick="startRotate()">Zu Heading drehen</button>
<button class="act-btn subtle full" id="rotate_stop_btn" onclick="stopRotate()" style="display:none;">Drehung abbrechen</button>
</div>
<div class="sr" id="rotate_status_row" style="display:none;">
<span class="sr-led led-blue" id="rotate_status_led"></span>
<span class="sr-key">Drehung</span>
<strong id="rotate_status_val" class="sr-val" style="font-family:var(--mono);white-space:pre;">-</strong>
</div>
<div class="sr">
<span class="sr-led led-off"></span>
@@ -963,6 +1020,10 @@
headingRawDeg: null,
headingCorrectedDeg: null,
headingOffsetDeg: 0,
rollRawDeg: null,
pitchRawDeg: null,
rollOffsetDeg: 0,
pitchOffsetDeg: 0,
error: '--',
lastDataAt: 0,
lastMagAt: 0,
@@ -1072,6 +1133,8 @@
imuRosState.rollPitchYaw = '--';
imuRosState.headingRawDeg = null;
imuRosState.headingCorrectedDeg = null;
imuRosState.rollRawDeg = null;
imuRosState.pitchRawDeg = null;
imuRosState.error = 'Keine ROS-Verbindung';
imuRosState.lastDataAt = 0;
imuRosState.lastMagAt = 0;
@@ -1350,6 +1413,11 @@
setText('imu_heading_raw', dataFresh ? formatHeadingDeg(imuRosState.headingRawDeg) : '--');
setText('imu_heading_corrected', dataFresh ? formatHeadingDeg(imuRosState.headingCorrectedDeg) : '--');
setText('imu_heading_offset', formatHeadingOffsetDeg(imuRosState.headingOffsetDeg));
var corrRoll = dataFresh && imuRosState.rollRawDeg !== null ? imuRosState.rollRawDeg + imuRosState.rollOffsetDeg : null;
var corrPitch = dataFresh && imuRosState.pitchRawDeg !== null ? imuRosState.pitchRawDeg + imuRosState.pitchOffsetDeg : null;
setText('imu_roll_corrected', corrRoll !== null ? corrRoll.toFixed(1) + '°' : '--');
setText('imu_pitch_corrected', corrPitch !== null ? corrPitch.toFixed(1) + '°' : '--');
setText('imu_tilt_offset', 'R ' + imuRosState.rollOffsetDeg.toFixed(1) + '° | P ' + imuRosState.pitchOffsetDeg.toFixed(1) + '°');
setText('imu_error', error || '-');
}
@@ -2663,6 +2731,8 @@
imuRosState.headingCorrectedDeg = Array.isArray(eulerDeg)
? normalizeHeadingDeg(eulerDeg[2] + Number(imuRosState.headingOffsetDeg || 0))
: null;
imuRosState.rollRawDeg = Array.isArray(eulerDeg) ? eulerDeg[0] : null;
imuRosState.pitchRawDeg = Array.isArray(eulerDeg) ? eulerDeg[1] : null;
imuRosState.lastDataAt = Date.now();
if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
imuRosState.error = '';
@@ -2712,6 +2782,11 @@
if (typeof imuHeading.offset_deg === 'number') imuRosState.headingOffsetDeg = imuHeading.offset_deg;
if (typeof imuHeading.raw_heading_deg === 'number') imuRosState.headingRawDeg = imuHeading.raw_heading_deg;
if (typeof imuHeading.corrected_heading_deg === 'number') imuRosState.headingCorrectedDeg = imuHeading.corrected_heading_deg;
var imuTilt = data.imu_tilt || {};
if (typeof imuTilt.roll_offset_deg === 'number') imuRosState.rollOffsetDeg = imuTilt.roll_offset_deg;
if (typeof imuTilt.pitch_offset_deg === 'number') imuRosState.pitchOffsetDeg = imuTilt.pitch_offset_deg;
if (typeof imuTilt.raw_roll_deg === 'number') imuRosState.rollRawDeg = imuTilt.raw_roll_deg;
if (typeof imuTilt.raw_pitch_deg === 'number') imuRosState.pitchRawDeg = imuTilt.raw_pitch_deg;
setAdminStatus(data.status || 'Bereit');
setText('info_ips', system.ips);
setText('info_wifi', system.wifi);
@@ -2786,6 +2861,27 @@
}
}
(function() {
try {
var h = localStorage.getItem('hudHexColor');
if (h) {
var el = document.getElementById('hud_color_picker');
if (el) el.value = '#' + h;
}
} catch(e) {}
})();
function applyHudColorFromAdmin(cssColor) {
var hex = cssColor.replace('#', '');
try { localStorage.setItem('hudHexColor', hex); } catch(e) {}
}
function resetHudColor() {
var el = document.getElementById('hud_color_picker');
if (el) el.value = '#00ff91';
applyHudColorFromAdmin('#00ff91');
}
async function setImuNorth() {
if (!window.confirm('Rover jetzt wirklich sauber nach Norden ausgerichtet?')) return;
setAdminStatus('Speichere Nordrichtung...');
@@ -2806,6 +2902,111 @@
}
}
async function setImuLevel() {
if (!window.confirm('Rover jetzt wirklich waagerecht aufgestellt?')) return;
setAdminStatus('Speichere Neigungsoffset...');
try {
var response = await fetch(adminApiBase + '/api/imu/set-level', { method: 'POST' });
var data = await response.json();
if (!response.ok) throw new Error(data.status || ('status ' + response.status));
var imuTilt = data.imu_tilt || {};
if (typeof imuTilt.roll_offset_deg === 'number') imuRosState.rollOffsetDeg = imuTilt.roll_offset_deg;
if (typeof imuTilt.pitch_offset_deg === 'number') imuRosState.pitchOffsetDeg = imuTilt.pitch_offset_deg;
if (typeof imuTilt.raw_roll_deg === 'number') imuRosState.rollRawDeg = imuTilt.raw_roll_deg;
if (typeof imuTilt.raw_pitch_deg === 'number') imuRosState.pitchRawDeg = imuTilt.raw_pitch_deg;
renderImuRosState();
setAdminStatus(data.status || 'Neigung genullt');
window.setTimeout(fetchStatus, 500);
} catch (error) {
setAdminStatus('Neigungskalibrierung fehlgeschlagen');
alert(error.message || 'Neigung konnte nicht genullt werden.');
}
}
var rotatePoller = null;
function updateRotateUi(rotate) {
if (!rotate) return;
var running = rotate.result === 'running';
var startBtn = document.getElementById('rotate_start_btn');
var stopBtn = document.getElementById('rotate_stop_btn');
var row = document.getElementById('rotate_status_row');
var val = document.getElementById('rotate_status_val');
var led = document.getElementById('rotate_status_led');
if (startBtn) startBtn.style.display = running ? 'none' : '';
if (stopBtn) stopBtn.style.display = running ? '' : 'none';
if (row) row.style.display = rotate.result === 'idle' ? 'none' : '';
if (led) {
led.className = 'sr-led ' + (
rotate.result === 'done' ? 'led-ok' :
rotate.result === 'error' ? 'led-warn' :
rotate.result === 'aborted' ? 'led-warn' : 'led-blue'
);
}
if (val) {
var txt = rotate.message || '-';
if (running && rotate.current_deg !== null && rotate.error_deg !== null) {
var cur = Number(rotate.current_deg).toFixed(1).padStart(6, ' ');
var rest = Number(rotate.error_deg).toFixed(1).padStart(7, ' ');
txt += ' | Ist:' + cur + '° | Rest:' + rest + '°';
}
val.textContent = txt;
}
if (!running && rotatePoller) {
clearInterval(rotatePoller);
rotatePoller = null;
try { localStorage.removeItem('autoRotateActive'); } catch(e) {}
}
}
async function startRotate() {
var input = document.getElementById('rotate_target');
var speedEl = document.getElementById('rotate_speed');
var target = parseFloat(input ? input.value : 0);
var speedPct = speedEl ? parseInt(speedEl.value) : 20;
if (isNaN(target)) { alert('Ungültiger Winkel'); return; }
setAdminStatus('Starte Drehung zu ' + target + '°…');
try {
var response = await fetch(adminApiBase + '/api/imu/rotate-to', {
method: 'POST',
headers: {'Content-Type': 'application/json'},
body: JSON.stringify({target_deg: target, speed_pct: speedPct})
});
var data = await response.json();
if (!response.ok) throw new Error(data.status || ('status ' + response.status));
try { localStorage.setItem('autoRotateActive', '1'); } catch(e) {}
updateRotateUi(data.rotate);
setAdminStatus(data.status || 'Drehung gestartet');
if (rotatePoller) clearInterval(rotatePoller);
rotatePoller = setInterval(pollRotateStatus, 300);
} catch (e) {
setAdminStatus('Fehler: ' + (e.message || 'unbekannt'));
alert(e.message || 'Drehung konnte nicht gestartet werden.');
}
}
async function stopRotate() {
setAdminStatus('Stoppe Drehung…');
try {
var response = await fetch(adminApiBase + '/api/imu/rotate-stop', { method: 'POST' });
var data = await response.json();
try { localStorage.removeItem('autoRotateActive'); } catch(e) {}
updateRotateUi(data.rotate);
setAdminStatus(data.status || 'Drehung gestoppt');
} catch (e) {
setAdminStatus('Fehler beim Stoppen');
}
}
async function pollRotateStatus() {
try {
var response = await fetch(adminApiBase + '/api/imu/rotate-status');
if (!response.ok) return;
var data = await response.json();
updateRotateUi(data.rotate);
} catch (e) {}
}
async function fetchDelay() {
try {
var response = await fetch(adminApiBase + '/api/delay');
+19 -7
View File
@@ -217,10 +217,15 @@ header {
background: rgba(0,255,145,.07);
}
.dbtn.active {
border-color: rgba(0,255,145,.7);
background: rgba(0,255,145,.1);
color: #00ff91;
box-shadow: 0 0 20px rgba(0,255,145,.25), inset 0 0 14px rgba(0,255,145,.06);
border-color: rgba(220,50,50,.75);
background: rgba(200,30,30,.12);
color: #ff6060;
box-shadow: 0 0 16px rgba(220,50,50,.25), inset 0 0 10px rgba(200,30,30,.07);
animation: active-pulse 2.8s ease-in-out infinite;
}
@keyframes active-pulse {
0%,100% { box-shadow: 0 0 10px rgba(220,50,50,.18), inset 0 0 8px rgba(200,30,30,.05); }
50% { box-shadow: 0 0 22px rgba(220,50,50,.45), inset 0 0 14px rgba(200,30,30,.12); }
}
@keyframes mode-flash {
0% { box-shadow: 0 0 0px rgba(0,255,145,0), background-color: var(--pri); }
@@ -281,10 +286,11 @@ header {
top: 50%;
left: 50%;
transform: translate(-50%, -50%);
width: calc(270px * var(--cam-scale));
height: calc(270px * var(--cam-scale));
width: calc(405px * var(--cam-scale));
height: calc(405px * var(--cam-scale));
z-index: 10;
pointer-events: none;
filter: drop-shadow(0 0 1.5px rgba(0,0,0,.9)) drop-shadow(0 0 3px rgba(0,0,0,.6));
}
@keyframes xh-spin { to { transform: rotate(360deg); } }
@keyframes xh-pulse {
@@ -543,7 +549,13 @@ body.page-connection-degraded .admin-shell{
transition: all .15s;
text-transform: uppercase;
}
.mtbtn.active { background: rgba(0,255,145,.1); color: #00ff91; border-color: rgba(0,255,145,.5); box-shadow: 0 0 12px rgba(0,255,145,.2); }
.mtbtn.active {
background: rgba(200,30,30,.12);
color: #ff6060;
border-color: rgba(220,50,50,.75);
box-shadow: 0 0 14px rgba(220,50,50,.25);
animation: active-pulse 2.8s ease-in-out infinite;
}
.drive-inline {
display: flex;
+98 -52
View File
@@ -94,7 +94,7 @@
S: <span id="overlay-distance">0,00</span> m
</div>
<div class="c-xhair">
<svg viewBox="-110 -110 220 220" xmlns="http://www.w3.org/2000/svg" width="220" height="220">
<svg viewBox="-110 -110 220 220" xmlns="http://www.w3.org/2000/svg" width="100%" height="100%">
<!-- outer rotating dashed ring -->
<circle class="xh-rot" cx="0" cy="0" r="103" fill="none" stroke="rgba(0,255,145,.14)" stroke-width="1" stroke-dasharray="4 13"/>
<!-- roll arc (static) r=96, ±60° from top -->
@@ -158,8 +158,8 @@
<line x1="-42" y1="-72" x2="42" y2="-72" stroke="rgba(0,255,145,.35)" stroke-width=".5"/>
<text id="imu-heading-text" x="0" y="-62.5" fill="rgba(0,255,145,.92)" font-size="9" text-anchor="middle" letter-spacing="1">HDG 000° N</text>
<!-- roll / pitch readouts -->
<text id="imu-roll-text" x="-73" y="76" fill="rgba(0,255,145,.82)" font-size="7.5" text-anchor="middle">R +0.0°</text>
<text id="imu-pitch-text" x="73" y="76" fill="rgba(0,255,145,.82)" font-size="7.5" text-anchor="middle">P +0.0°</text>
<text id="imu-roll-text" x="-73" y="76" fill="rgba(0,255,145,.82)" font-size="10.5" text-anchor="middle">R +0.0°</text>
<text id="imu-pitch-text" x="73" y="76" fill="rgba(0,255,145,.82)" font-size="10.5" text-anchor="middle">P +0.0°</text>
</g>
<!-- center dot -->
<circle cx="0" cy="0" r="3" fill="rgba(0,255,145,.92)"/>
@@ -167,7 +167,6 @@
</svg>
</div>
<div class="c-scanlines"></div>
<div class="c-vignette"></div>
</div>
</div>
</div>
@@ -324,9 +323,31 @@ var imuOverlayState = {
pitchDeg: 0,
yawDeg: 0,
yawOffsetDeg: 0,
rollOffsetDeg: 0,
pitchOffsetDeg: 0,
lastUpdateAt: 0
};
var hudRGB = [0, 255, 145];
var xhairSvgOrigHTML = null;
function hudRgba(alpha) {
return 'rgba(' + hudRGB[0] + ',' + hudRGB[1] + ',' + hudRGB[2] + ',' + alpha + ')';
}
function applyHudColor(hex) {
var r = parseInt(hex.slice(0, 2), 16);
var g = parseInt(hex.slice(2, 4), 16);
var b = parseInt(hex.slice(4, 6), 16);
if (isNaN(r) || isNaN(g) || isNaN(b)) return;
hudRGB = [r, g, b];
try { localStorage.setItem('hudHexColor', hex); } catch (e) {}
var svgEl = document.querySelector('.c-xhair svg');
if (!svgEl) return;
if (!xhairSvgOrigHTML) xhairSvgOrigHTML = svgEl.innerHTML;
svgEl.innerHTML = xhairSvgOrigHTML.replace(/rgba\(0,255,145,/g, 'rgba(' + r + ',' + g + ',' + b + ',');
}
function setText(id, text) {
var el = document.getElementById(id);
if (el) {
@@ -355,7 +376,7 @@ function formatSignedNumber(value, digits) {
}
function headingToCompass(deg) {
var dirs = ["N","NNE","NE","ENE","E","ESE","SE","SSE","S","SSW","SW","WSW","W","WNW","NW","NNW"];
var dirs = ["N","NNO","NO","ONO","O","OSO","SO","SSO","S","SSW","SW","WSW","W","WNW","NW","NNW"];
return dirs[Math.round(((deg % 360) + 360) % 360 / 22.5) % 16];
}
@@ -806,8 +827,10 @@ function setRosStatus(text) {
setText("ros-state", text);
}
var autoRotateActive = false;
function controlsAreBlocked() {
return pageConnection.state === "offline";
return pageConnection.state === "offline" || autoRotateActive;
}
function updateControlAvailability() {
@@ -992,12 +1015,19 @@ function updateServiceState(payload) {
var system = payload.system || {};
var services = payload.services || {};
var imuHeading = payload.imu_heading || {};
var imuTilt = payload.imu_tilt || {};
var cameraServiceState = String(services.camera || "");
var cameraSettingsVersion = Number(services.camera_settings_version || 0);
if (typeof imuHeading.offset_deg === "number" && !isNaN(imuHeading.offset_deg)) {
imuOverlayState.yawOffsetDeg = Number(imuHeading.offset_deg);
updateImuOverlaySvg();
}
if (typeof imuTilt.roll_offset_deg === "number" && !isNaN(imuTilt.roll_offset_deg)) {
imuOverlayState.rollOffsetDeg = Number(imuTilt.roll_offset_deg);
}
if (typeof imuTilt.pitch_offset_deg === "number" && !isNaN(imuTilt.pitch_offset_deg)) {
imuOverlayState.pitchOffsetDeg = Number(imuTilt.pitch_offset_deg);
}
updateImuOverlaySvg();
updateUndervoltageDisplay(system.undervoltage || "unbekannt", system.undervoltage_state || "unknown");
if (cameraServiceState === "active" && lastCameraServiceState && lastCameraServiceState !== "active") {
setText("cam-state", "Verbinde...");
@@ -1132,12 +1162,14 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
context.save();
context.translate(cx, cy);
context.rotate(-rollRad);
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 3;
context.beginPath();
context.moveTo(0, -90 * unit);
context.lineTo(-5.5 * unit, -79 * unit);
context.lineTo(5.5 * unit, -79 * unit);
context.closePath();
context.fillStyle = "rgba(0,255,145,.88)";
context.fillStyle = hudRgba(.88);
context.fill();
context.restore();
@@ -1146,6 +1178,8 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
context.translate(cx, cy);
context.rotate(-rollRad);
context.translate(0, pitchOffsetPx);
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 2.5;
[
{ y: -33, span: 11, op: .28 }, { y: 33, span: 11, op: .28 },
@@ -1155,12 +1189,12 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
context.beginPath();
context.moveTo(-pl.span * unit, pl.y * unit);
context.lineTo(pl.span * unit, pl.y * unit);
context.strokeStyle = "rgba(0,255,145," + pl.op + ")";
context.strokeStyle = hudRgba(pl.op);
context.lineWidth = 0.9 * unit;
context.stroke();
});
context.strokeStyle = "rgba(0,255,145,.95)";
context.strokeStyle = hudRgba(.95);
context.lineWidth = 2.8 * unit;
context.beginPath(); context.moveTo(-60 * unit, 0); context.lineTo(60 * unit, 0); context.stroke();
context.beginPath(); context.moveTo(-60 * unit, 0); context.lineTo(-52 * unit, -5 * unit); context.stroke();
@@ -1172,13 +1206,15 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
var bw = 84 * unit, bh = 15 * unit;
context.fillStyle = "rgba(0,15,8,.65)";
context.fillRect(cx - bw / 2, cy - 72 * unit, bw, bh);
context.shadowColor = "rgba(0,0,0,.9)";
context.shadowBlur = 3;
context.font = (9 * unit) + 'px "Consolas","Cascadia Mono",monospace';
context.fillStyle = "rgba(0,255,145,.92)";
context.fillStyle = hudRgba(.92);
context.textAlign = "center";
context.textBaseline = "middle";
context.fillText("HDG " + String(Math.round(headingDeg)).padStart(3, "0") + "° " + headingToCompass(headingDeg), cx, cy - 62.5 * unit);
context.font = (7.5 * unit) + 'px "Consolas","Cascadia Mono",monospace';
context.fillStyle = "rgba(0,255,145,.72)";
context.font = (10.5 * unit) + 'px "Consolas","Cascadia Mono",monospace';
context.fillStyle = hudRgba(.82);
context.fillText("R " + formatSignedNumber(rollDeg, 1) + "°", cx - 73 * unit, cy + 76 * unit);
context.fillText("P " + formatSignedNumber(pitchDeg, 1) + "°", cx + 73 * unit, cy + 76 * unit);
context.restore();
@@ -1189,67 +1225,76 @@ function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size)
var cy = canvasHeight / 2;
var unit = size / 180;
function circle(radius, stroke, lineWidth, dash) {
function circle(radius, alpha, lineWidth, dash) {
context.save();
context.beginPath();
context.arc(cx, cy, radius * unit, 0, Math.PI * 2);
if (dash) context.setLineDash(dash.map(function (value) { return value * unit; }));
context.strokeStyle = stroke;
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 2.5;
context.strokeStyle = hudRgba(alpha);
context.lineWidth = lineWidth * unit;
context.stroke();
context.restore();
}
function line(x1, y1, x2, y2, stroke, lineWidth) {
function line(x1, y1, x2, y2, alpha, lineWidth) {
context.save();
context.beginPath();
context.moveTo(cx + (x1 * unit), cy + (y1 * unit));
context.lineTo(cx + (x2 * unit), cy + (y2 * unit));
context.strokeStyle = stroke;
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 2.5;
context.strokeStyle = hudRgba(alpha);
context.lineWidth = lineWidth * unit;
context.stroke();
context.restore();
}
function path(points, stroke, lineWidth) {
function path(points, alpha, lineWidth) {
context.save();
context.beginPath();
context.moveTo(cx + (points[0][0] * unit), cy + (points[0][1] * unit));
for (var i = 1; i < points.length; i += 1) {
context.lineTo(cx + (points[i][0] * unit), cy + (points[i][1] * unit));
}
context.strokeStyle = stroke;
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 2.5;
context.strokeStyle = hudRgba(alpha);
context.lineWidth = lineWidth * unit;
context.stroke();
context.restore();
}
circle(82, "rgba(0,255,145,.2)", 1, [4, 10]);
circle(60, "rgba(0,255,145,.15)", 0.75, [2, 16]);
circle(16, "rgba(0,255,145,.3)", 0.75);
line(-86, 0, -19, 0, "rgba(0,255,145,.6)", 1);
line(19, 0, 86, 0, "rgba(0,255,145,.6)", 1);
line(0, -86, 0, -19, "rgba(0,255,145,.6)", 1);
line(0, 19, 0, 86, "rgba(0,255,145,.6)", 1);
line(-60, -6, -60, 6, "rgba(0,255,145,.45)", 0.75);
line(60, -6, 60, 6, "rgba(0,255,145,.45)", 0.75);
line(-6, -60, 6, -60, "rgba(0,255,145,.45)", 0.75);
line(-6, 60, 6, 60, "rgba(0,255,145,.45)", 0.75);
line(-38, -3.5, -38, 3.5, "rgba(0,255,145,.3)", 0.5);
line(38, -3.5, 38, 3.5, "rgba(0,255,145,.3)", 0.5);
line(-3.5, -38, 3.5, -38, "rgba(0,255,145,.3)", 0.5);
line(-3.5, 38, 3.5, 38, "rgba(0,255,145,.3)", 0.5);
path([[-74, -58], [-74, -74], [-58, -74]], "rgba(0,255,145,.75)", 1.5);
path([[58, -74], [74, -74], [74, -58]], "rgba(0,255,145,.75)", 1.5);
path([[-74, 58], [-74, 74], [-58, 74]], "rgba(0,255,145,.75)", 1.5);
path([[58, 74], [74, 74], [74, 58]], "rgba(0,255,145,.75)", 1.5);
circle(6, "rgba(0,255,145,.7)", 1);
circle(82, .2, 1, [4, 10]);
circle(60, .15, 0.75, [2, 16]);
circle(16, .3, 0.75);
line(-86, 0, -19, 0, .6, 1);
line(19, 0, 86, 0, .6, 1);
line(0, -86, 0, -19, .6, 1);
line(0, 19, 0, 86, .6, 1);
line(-60, -6, -60, 6, .45, 0.75);
line(60, -6, 60, 6, .45, 0.75);
line(-6, -60, 6, -60, .45, 0.75);
line(-6, 60, 6, 60, .45, 0.75);
line(-38, -3.5, -38, 3.5, .3, 0.5);
line(38, -3.5, 38, 3.5, .3, 0.5);
line(-3.5, -38, 3.5, -38, .3, 0.5);
line(-3.5, 38, 3.5, 38, .3, 0.5);
path([[-74, -58], [-74, -74], [-58, -74]], .75, 1.5);
path([[58, -74], [74, -74], [74, -58]], .75, 1.5);
path([[-74, 58], [-74, 74], [-58, 74]], .75, 1.5);
path([[58, 74], [74, 74], [74, 58]], .75, 1.5);
circle(6, .7, 1);
context.save();
context.shadowColor = "rgba(0,0,0,.85)";
context.shadowBlur = 3;
context.beginPath();
context.arc(cx, cy, 2.5 * unit, 0, Math.PI * 2);
context.fillStyle = "rgba(0,255,145,.9)";
context.fillStyle = hudRgba(.9);
context.fill();
context.shadowBlur = 0;
context.beginPath();
context.arc(cx, cy, 1 * unit, 0, Math.PI * 2);
context.fillStyle = "rgba(255,255,255,.7)";
@@ -1262,17 +1307,9 @@ function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size)
function drawVignetteAndScanlines(context, canvasWidth, canvasHeight) {
context.save();
for (var y = 0; y < canvasHeight; y += 4) {
context.fillStyle = "rgba(0,255,145,.012)";
context.fillStyle = hudRgba(.012);
context.fillRect(0, y, canvasWidth, 1);
}
var vignette = context.createRadialGradient(
canvasWidth * 0.5, canvasHeight * 0.5, canvasWidth * 0.25,
canvasWidth * 0.5, canvasHeight * 0.5, canvasWidth * 0.75
);
vignette.addColorStop(0, "rgba(0,0,0,0)");
vignette.addColorStop(1, "rgba(0,0,0,.58)");
context.fillStyle = vignette;
context.fillRect(0, 0, canvasWidth, canvasHeight);
context.restore();
}
@@ -1330,7 +1367,7 @@ function drawHudOverlayToCanvas(context, canvasWidth, canvasHeight) {
context.fillText(document.getElementById("cam-live-text").textContent, canvasWidth / 2 + (6 * camScale * scaleToCanvas), padding);
context.restore();
drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, 270 * camScale * scaleToCanvas);
drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, 405 * camScale * scaleToCanvas);
drawVignetteAndScanlines(context, canvasWidth, canvasHeight);
}
@@ -1535,7 +1572,16 @@ function toggleStatPanel() {
setStatPanelCollapsed(!panel.classList.contains("collapsed"));
}
window.addEventListener("storage", function (e) {
if (e.key === "hudHexColor" && e.newValue) applyHudColor(e.newValue);
if (e.key === "autoRotateActive") {
autoRotateActive = e.newValue === "1";
if (!autoRotateActive) { setAxes(0, 0); }
}
});
window.addEventListener("load", function () {
try { var h = localStorage.getItem("hudHexColor"); if (h) applyHudColor(h); } catch (e) {}
setText("cam-host", hostUrl);
updateModeDisplay();
updateMotorDisplay();
@@ -1709,8 +1755,8 @@ window.addEventListener("load", function () {
Number(orientation.z || 0),
Number(orientation.w || 1)
]);
imuOverlayState.rollDeg = euler.rollDeg;
imuOverlayState.pitchDeg = euler.pitchDeg;
imuOverlayState.rollDeg = euler.rollDeg + imuOverlayState.rollOffsetDeg;
imuOverlayState.pitchDeg = euler.pitchDeg + imuOverlayState.pitchOffsetDeg;
imuOverlayState.yawDeg = euler.yawDeg;
imuOverlayState.lastUpdateAt = Date.now();
updateImuOverlaySvg();
+119
View File
@@ -0,0 +1,119 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Laeuft INNERHALB des Docker-Containers mit nativem rospy.
Dreht den Rover auf der Stelle zu einem Ziel-Heading und beendet sich.
Aufruf:
python3 auto_rotate_ros.py <target_deg> <restore_mode> <heading_offset_deg>
Exit-Codes:
0 = Ziel erreicht
1 = Fehler
"""
import math
import signal
import sys
import time
import rospy
from exomy.msg import RoverCommand
from sensor_msgs.msg import Imu
_POINT_TURN = 2
_DONE_DEG = float(sys.argv[4]) if len(sys.argv) > 4 else 1.0
_VEL = int(sys.argv[5]) if len(sys.argv) > 5 else 20
target_deg = float(sys.argv[1]) % 360
restore_mode = int(sys.argv[2])
heading_offset = float(sys.argv[3])
_running = True
_publisher = None
_heading = None
def _normalize(deg):
return (float(deg) % 360 + 360) % 360
def _shortest_error(current, target):
return (target - current + 540) % 360 - 180
def _on_imu(msg):
global _heading
o = msg.orientation
x, y, z, w = o.x, o.y, o.z, o.w
siny = 2.0 * (w * z + x * y)
cosy = 1.0 - 2.0 * (y * y + z * z)
yaw_deg = math.degrees(math.atan2(siny, cosy))
_heading = _normalize(yaw_deg + heading_offset)
def _publish(locomotion_mode, vel, steering=0):
if _publisher is None:
return
cmd = RoverCommand()
cmd.connected = True
cmd.motors_enabled = True
cmd.locomotion_mode = locomotion_mode
cmd.vel = vel
cmd.steering = steering
_publisher.publish(cmd)
def _shutdown(signum, frame):
global _running
_running = False
def main():
global _publisher, _running
signal.signal(signal.SIGTERM, _shutdown)
signal.signal(signal.SIGINT, _shutdown)
rospy.init_node('auto_rotate', anonymous=True, disable_signals=True)
_publisher = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
rospy.Subscriber('/imu/data', Imu, _on_imu, queue_size=1)
rospy.set_param('/auto_rotate_active', True)
# Kurz warten bis IMU-Daten ankommen
deadline = time.time() + 5.0
while _heading is None and time.time() < deadline and _running:
time.sleep(0.05)
if _heading is None:
rospy.logerr('auto_rotate: keine IMU-Daten')
rospy.set_param('/auto_rotate_active', False)
_publish(restore_mode, vel=0)
sys.exit(1)
rate = rospy.Rate(50) # 50 Hz, passend zum PWM-Chip
while _running and not rospy.is_shutdown():
error = _shortest_error(_heading, target_deg)
if abs(error) <= _DONE_DEG:
rospy.set_param('/auto_rotate_active', False)
_publish(restore_mode, vel=0)
sys.exit(0)
# Richtung wie Joystick: immer positiver vel, steering ±180 fuer Richtung
if error > 0:
_publish(_POINT_TURN, vel=_VEL, steering=0)
else:
_publish(_POINT_TURN, vel=_VEL, steering=180)
rate.sleep()
# Abbruch per Signal
rospy.set_param('/auto_rotate_active', False)
_publish(restore_mode, vel=0)
sys.exit(0)
if __name__ == '__main__':
main()
+156 -2
View File
@@ -32,6 +32,9 @@ CAMERA_RUNTIME_SETTINGS_FILE = '/tmp/exomy_camera_settings.json'
IMU_HEADING_CALIBRATION_FILE = os.path.join(
os.path.dirname(__file__), '..', 'config', 'imu_heading_calibration.json'
)
IMU_TILT_CALIBRATION_FILE = os.path.join(
os.path.dirname(__file__), '..', 'config', 'imu_tilt_calibration.json'
)
MOTOR_TEST_LOCK = threading.Lock()
GPS_PORT_PATTERNS = [
'/dev/serial/by-id/*',
@@ -82,6 +85,7 @@ IMU_ROS_LOCK = threading.Lock()
IMU_ROS_THREAD = None
IMU_ROS_CLIENT = None
IMU_ROS_TOPICS = []
LAST_LOCOMOTION_MODE = 1 # ACKERMANN als Fallback
IMU_ROS_CACHE = {
'state': 'unknown',
'status': 'Warte auf ROS',
@@ -91,6 +95,8 @@ IMU_ROS_CACHE = {
'magnetic': 'unbekannt',
'quaternion': 'unbekannt',
'roll_pitch_yaw': 'unbekannt',
'roll_deg': None,
'pitch_deg': None,
'error': 'Noch keine IMU-Daten aus ROS',
'heading_deg': None,
'updated_at': 0.0,
@@ -195,6 +201,8 @@ def _on_imu_data_message(message):
), 'rad/s', 3),
quaternion=_format_quaternion(quaternion),
roll_pitch_yaw=_format_euler_deg(euler_deg),
roll_deg=None if not euler_deg else euler_deg[0],
pitch_deg=None if not euler_deg else euler_deg[1],
heading_deg=None if not euler_deg else _normalize_heading_deg(euler_deg[2]),
error='',
)
@@ -220,6 +228,7 @@ def _connect_imu_ros():
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
import roslibpy
import imu_rotate
client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT)
client.run()
@@ -232,14 +241,26 @@ def _connect_imu_ros():
imu_topic = roslibpy.Topic(client, '/imu/data', 'sensor_msgs/Imu')
mag_topic = roslibpy.Topic(client, '/imu/mag', 'sensor_msgs/MagneticField')
status_topic = roslibpy.Topic(client, '/imu/status', 'std_msgs/String')
rover_cmd_topic = roslibpy.Topic(client, '/rover_command', 'exomy/RoverCommand')
def _on_rover_cmd_message(message):
global LAST_LOCOMOTION_MODE
import imu_rotate
if not imu_rotate.get_rotate_status()['running']:
mode = message.get('locomotion_mode')
if mode is not None:
LAST_LOCOMOTION_MODE = int(mode)
imu_topic.subscribe(_on_imu_data_message)
mag_topic.subscribe(_on_imu_mag_message)
status_topic.subscribe(_on_imu_status_message)
rover_cmd_topic.subscribe(_on_rover_cmd_message)
rover_cmd_topic.advertise()
IMU_ROS_CLIENT = client
IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic]
IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic, rover_cmd_topic]
_imu_ros_cache_update(status='ROS verbunden', error='')
def _imu_ros_worker():
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
@@ -856,6 +877,7 @@ def collect_status():
container_details = get_container_details(EXOMY_CONTAINER)
imu_details = read_imu_ros_status()
imu_heading = get_imu_heading_status()
imu_tilt = get_imu_tilt_status()
return {
'status': 'Bereit',
'system': {
@@ -899,6 +921,7 @@ def collect_status():
},
'imu': imu_details,
'imu_heading': imu_heading,
'imu_tilt': imu_tilt,
}
@@ -1142,6 +1165,85 @@ def calibrate_imu_heading_to_north():
}
def get_default_imu_tilt_calibration():
return {
'roll_offset_deg': 0.0,
'pitch_offset_deg': 0.0,
'updated_at': None,
'source': 'default',
}
def read_imu_tilt_calibration():
data = get_default_imu_tilt_calibration()
try:
with open(IMU_TILT_CALIBRATION_FILE, 'r', encoding='utf-8') as handle:
payload = json.load(handle)
data['roll_offset_deg'] = float(payload.get('roll_offset_deg', 0.0))
data['pitch_offset_deg'] = float(payload.get('pitch_offset_deg', 0.0))
data['updated_at'] = payload.get('updated_at')
data['source'] = payload.get('source', 'file')
except (OSError, ValueError, KeyError):
pass
return data
def write_imu_tilt_calibration(roll_offset_deg, pitch_offset_deg):
payload = {
'roll_offset_deg': float(roll_offset_deg),
'pitch_offset_deg': float(pitch_offset_deg),
'updated_at': time.strftime('%Y-%m-%dT%H:%M:%S'),
'source': 'admin',
}
os.makedirs(os.path.dirname(IMU_TILT_CALIBRATION_FILE), exist_ok=True)
with open(IMU_TILT_CALIBRATION_FILE, 'w', encoding='utf-8') as handle:
json.dump(payload, handle, indent=2)
return payload
def get_imu_tilt_status():
calibration = read_imu_tilt_calibration()
with IMU_ROS_LOCK:
raw_roll_deg = IMU_ROS_CACHE.get('roll_deg')
raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg')
corrected_roll = None if raw_roll_deg is None else raw_roll_deg + calibration['roll_offset_deg']
corrected_pitch = None if raw_pitch_deg is None else raw_pitch_deg + calibration['pitch_offset_deg']
return {
'roll_offset_deg': calibration['roll_offset_deg'],
'pitch_offset_deg': calibration['pitch_offset_deg'],
'raw_roll_deg': raw_roll_deg,
'raw_pitch_deg': raw_pitch_deg,
'corrected_roll_deg': corrected_roll,
'corrected_pitch_deg': corrected_pitch,
'updated_at': calibration['updated_at'],
'source': calibration['source'],
}
def zero_imu_tilt():
with IMU_ROS_LOCK:
raw_roll_deg = IMU_ROS_CACHE.get('roll_deg')
raw_pitch_deg = IMU_ROS_CACHE.get('pitch_deg')
if raw_roll_deg is None or raw_pitch_deg is None:
return 409, {'status': 'Noch keine frischen IMU-Daten für Neigungskalibrierung'}
payload = write_imu_tilt_calibration(-float(raw_roll_deg), -float(raw_pitch_deg))
return 200, {
'status': 'Neigung genullt',
'imu_tilt': {
'roll_offset_deg': payload['roll_offset_deg'],
'pitch_offset_deg': payload['pitch_offset_deg'],
'raw_roll_deg': raw_roll_deg,
'raw_pitch_deg': raw_pitch_deg,
'corrected_roll_deg': 0.0,
'corrected_pitch_deg': 0.0,
'updated_at': payload['updated_at'],
'source': payload['source'],
},
}
def set_camera_config(profile_id=None, fps=None):
current = read_camera_settings()
if profile_id is None:
@@ -1869,6 +1971,20 @@ class Handler(http.server.BaseHTTPRequestHandler):
self.end_headers()
def do_GET(self):
if self.path == '/api/imu/rotate-status':
import imu_rotate
status = imu_rotate.get_rotate_status()
if status.get('running') and status.get('target_deg') is not None:
calibration = read_imu_heading_calibration()
with IMU_ROS_LOCK:
raw = IMU_ROS_CACHE.get('heading_deg')
corrected = get_corrected_heading_deg(raw, calibration['offset_deg']) if raw is not None else None
status['current_deg'] = round(corrected, 1) if corrected is not None else None
if corrected is not None:
err = (status['target_deg'] - corrected + 540) % 360 - 180
status['error_deg'] = round(err, 1)
self.write_json(200, {'rotate': status})
return
if self.path == '/api/delay':
self.write_json(200, {'delay_seconds': get_delay()})
return
@@ -1911,7 +2027,8 @@ class Handler(http.server.BaseHTTPRequestHandler):
def do_POST(self):
raw_body = b'{}'
if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset'):
if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset',
'/api/imu/rotate-to'):
try:
content_length = int(self.headers.get('Content-Length', '0'))
except ValueError:
@@ -2197,6 +2314,43 @@ class Handler(http.server.BaseHTTPRequestHandler):
self.write_json(status_code, response)
return
if self.path == '/api/imu/set-level':
status_code, response = zero_imu_tilt()
self.write_json(status_code, response)
return
if self.path == '/api/imu/rotate-to':
try:
payload = json.loads(raw_body.decode('utf-8'))
target_deg = float(payload.get('target_deg', 0))
speed_pct = max(20, min(50, int(payload.get('speed_pct', 20))))
except (UnicodeDecodeError, json.JSONDecodeError, TypeError, ValueError):
self.write_json(400, {'status': 'Ungültige Anfrage'})
return
with IMU_ROS_LOCK:
heading = IMU_ROS_CACHE.get('heading_deg')
if heading is None:
self.write_json(409, {'status': 'Noch keine frischen IMU-Daten'})
return
import imu_rotate
heading_offset = read_imu_heading_calibration()['offset_deg']
vel = max(1, int(100.0 * (speed_pct / 100.0) ** 2.0))
status = imu_rotate.start_rotate(
target_deg=target_deg,
heading_offset=heading_offset,
restore_mode=LAST_LOCOMOTION_MODE,
vel=vel,
)
self.write_json(200, {'status': 'Drehung gestartet', 'rotate': status})
return
if self.path == '/api/imu/rotate-stop':
import imu_rotate
imu_rotate.stop_rotate()
self.write_json(200, {'status': 'Drehung gestoppt', 'rotate': imu_rotate.get_rotate_status()})
return
actions = {
'/api/cold-start-gps': {
'handler': cold_start_gps_receiver,
+112
View File
@@ -0,0 +1,112 @@
"""
Startet auto_rotate_ros.py via docker exec im ExoMy-Container.
Das Script läuft dort nativ mit rospy — kein WebSocket-Overhead.
Öffentliche API:
start_rotate(target_deg, heading_offset, restore_mode) -> dict
stop_rotate()
get_rotate_status() -> dict
"""
import subprocess
import threading
import time
_CONTAINER = 'exomy_autostart'
_SCRIPT_PATH = '/root/exomy_ws/src/exomy/scripts/auto_rotate_ros.py'
_ROS_SETUP = 'source /opt/ros/melodic/setup.bash && source /root/exomy_ws/devel/setup.bash'
_lock = threading.Lock()
_proc = None # subprocess.Popen (docker exec)
_monitor_thread = None
_status = {
'running': False,
'target_deg': None,
'result': 'idle',
'message': '',
}
def _monitor(proc, target_deg, restore_mode):
"""Wartet auf Prozessende und aktualisiert Status."""
global _proc
returncode = proc.wait()
with _lock:
if _proc is proc: # nicht überschrieben durch neuen Start
_proc = None
_status['running'] = False
if returncode == 0:
_status['result'] = 'done'
_status['message'] = 'Ziel erreicht'
elif returncode == -15 or returncode == 143:
_status['result'] = 'aborted'
_status['message'] = 'Abgebrochen'
else:
_status['result'] = 'error'
_status['message'] = 'Fehler (exit {})'.format(returncode)
def start_rotate(target_deg, heading_offset, restore_mode=1, done_deg=1.0, vel=None):
global _proc, _monitor_thread, _status
stop_rotate()
target_deg = float(target_deg) % 360
if vel is None:
# Fest 20% Geschwindigkeit, unabhängig vom Admin-Speed-Limit
# Entspricht dem Joystick-Expo: 100 * (20/100)^2 = 4
vel = 4
cmd = [
'docker', 'exec', _CONTAINER, 'bash', '-c',
'{setup} && python {script} {target} {mode} {offset} {done} {vel}'.format(
setup=_ROS_SETUP,
script=_SCRIPT_PATH,
target=target_deg,
mode=int(restore_mode),
offset=float(heading_offset),
done=float(done_deg),
vel=int(vel),
)
]
proc = subprocess.Popen(cmd, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
with _lock:
_proc = proc
_status = {
'running': True,
'target_deg': round(target_deg, 1),
'result': 'running',
'message': 'Drehe zu {:.0f}°'.format(target_deg),
}
_monitor_thread = threading.Thread(
target=_monitor,
args=(proc, target_deg, restore_mode),
daemon=True,
)
_monitor_thread.start()
return get_rotate_status()
def stop_rotate():
global _proc
with _lock:
proc = _proc
_proc = None
if proc is not None and proc.poll() is None:
# SIGTERM an den docker-exec-Prozess → Container-Prozess beendet sich sauber
proc.terminate()
try:
proc.wait(timeout=3.0)
except subprocess.TimeoutExpired:
proc.kill()
def get_rotate_status():
with _lock:
return dict(_status)
+9 -8
View File
@@ -141,24 +141,25 @@ def publish_measurements(imu_topic, mag_topic, status_topic, sensor):
magnetic = sensor.magnetic
quaternion = sensor.quaternion
# Sensor ist um 90° um die Z-Achse verdreht eingebaut → X- und Y-Achse tauschen
imu_topic.publish(roslibpy.Message({
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
'orientation': {
'x': quaternion[0],
'y': quaternion[1],
'x': quaternion[1],
'y': quaternion[0],
'z': quaternion[2],
'w': quaternion[3],
},
'orientation_covariance': [0.0] * 9,
'angular_velocity': {
'x': gyro[0],
'y': gyro[1],
'x': gyro[1],
'y': gyro[0],
'z': gyro[2],
},
'angular_velocity_covariance': [0.0] * 9,
'linear_acceleration': {
'x': acceleration[0],
'y': acceleration[1],
'x': acceleration[1],
'y': acceleration[0],
'z': acceleration[2],
},
'linear_acceleration_covariance': [0.0] * 9,
@@ -167,8 +168,8 @@ def publish_measurements(imu_topic, mag_topic, status_topic, sensor):
mag_topic.publish(roslibpy.Message({
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
'magnetic_field': {
'x': magnetic[0] * 1e-6,
'y': magnetic[1] * 1e-6,
'x': magnetic[1] * 1e-6,
'y': magnetic[0] * 1e-6,
'z': magnetic[2] * 1e-6,
},
'magnetic_field_covariance': [0.0] * 9,
+3
View File
@@ -180,6 +180,9 @@ def callback(data):
global last_webgui_time
global last_locomotion_mode
if rospy.get_param('/auto_rotate_active', False):
return
is_webgui = data.header.frame_id == "webgui"
now = rospy.Time.now()