Compare commits
10
Commits
e7d7d2ae20
...
9218b95e6e
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
9218b95e6e | ||
|
|
0c4cafd12b | ||
|
|
13af8052da | ||
|
|
ff58356c23 | ||
|
|
01e7d47b4e | ||
|
|
93772ec3b7 | ||
|
|
4c24558516 | ||
|
|
5e39a41f49 | ||
|
|
b4ea4799a9 | ||
|
|
5bfd5797fb |
@@ -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.
|
- 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.
|
- 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
|
### IMU-Topics
|
||||||
|
|
||||||
| Topic | Typ | Inhalt |
|
| 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
|
## Fahrmodi
|
||||||
|
|
||||||
| Modus | Taste (Controller) | Taste (Web-GUI) |
|
| Modus | Taste (Controller) | Taste (Web-GUI) |
|
||||||
|
|||||||
+201
@@ -153,8 +153,65 @@
|
|||||||
<span class="sr-key">Nord-Offset</span>
|
<span class="sr-key">Nord-Offset</span>
|
||||||
<strong id="imu_heading_offset" class="sr-val">-</strong>
|
<strong id="imu_heading_offset" class="sr-val">-</strong>
|
||||||
</div>
|
</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">
|
<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="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>
|
||||||
<div class="sr">
|
<div class="sr">
|
||||||
<span class="sr-led led-off"></span>
|
<span class="sr-led led-off"></span>
|
||||||
@@ -963,6 +1020,10 @@
|
|||||||
headingRawDeg: null,
|
headingRawDeg: null,
|
||||||
headingCorrectedDeg: null,
|
headingCorrectedDeg: null,
|
||||||
headingOffsetDeg: 0,
|
headingOffsetDeg: 0,
|
||||||
|
rollRawDeg: null,
|
||||||
|
pitchRawDeg: null,
|
||||||
|
rollOffsetDeg: 0,
|
||||||
|
pitchOffsetDeg: 0,
|
||||||
error: '--',
|
error: '--',
|
||||||
lastDataAt: 0,
|
lastDataAt: 0,
|
||||||
lastMagAt: 0,
|
lastMagAt: 0,
|
||||||
@@ -1072,6 +1133,8 @@
|
|||||||
imuRosState.rollPitchYaw = '--';
|
imuRosState.rollPitchYaw = '--';
|
||||||
imuRosState.headingRawDeg = null;
|
imuRosState.headingRawDeg = null;
|
||||||
imuRosState.headingCorrectedDeg = null;
|
imuRosState.headingCorrectedDeg = null;
|
||||||
|
imuRosState.rollRawDeg = null;
|
||||||
|
imuRosState.pitchRawDeg = null;
|
||||||
imuRosState.error = 'Keine ROS-Verbindung';
|
imuRosState.error = 'Keine ROS-Verbindung';
|
||||||
imuRosState.lastDataAt = 0;
|
imuRosState.lastDataAt = 0;
|
||||||
imuRosState.lastMagAt = 0;
|
imuRosState.lastMagAt = 0;
|
||||||
@@ -1350,6 +1413,11 @@
|
|||||||
setText('imu_heading_raw', dataFresh ? formatHeadingDeg(imuRosState.headingRawDeg) : '--');
|
setText('imu_heading_raw', dataFresh ? formatHeadingDeg(imuRosState.headingRawDeg) : '--');
|
||||||
setText('imu_heading_corrected', dataFresh ? formatHeadingDeg(imuRosState.headingCorrectedDeg) : '--');
|
setText('imu_heading_corrected', dataFresh ? formatHeadingDeg(imuRosState.headingCorrectedDeg) : '--');
|
||||||
setText('imu_heading_offset', formatHeadingOffsetDeg(imuRosState.headingOffsetDeg));
|
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 || '-');
|
setText('imu_error', error || '-');
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2663,6 +2731,8 @@
|
|||||||
imuRosState.headingCorrectedDeg = Array.isArray(eulerDeg)
|
imuRosState.headingCorrectedDeg = Array.isArray(eulerDeg)
|
||||||
? normalizeHeadingDeg(eulerDeg[2] + Number(imuRosState.headingOffsetDeg || 0))
|
? normalizeHeadingDeg(eulerDeg[2] + Number(imuRosState.headingOffsetDeg || 0))
|
||||||
: null;
|
: null;
|
||||||
|
imuRosState.rollRawDeg = Array.isArray(eulerDeg) ? eulerDeg[0] : null;
|
||||||
|
imuRosState.pitchRawDeg = Array.isArray(eulerDeg) ? eulerDeg[1] : null;
|
||||||
imuRosState.lastDataAt = Date.now();
|
imuRosState.lastDataAt = Date.now();
|
||||||
if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
|
if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') {
|
||||||
imuRosState.error = '';
|
imuRosState.error = '';
|
||||||
@@ -2712,6 +2782,11 @@
|
|||||||
if (typeof imuHeading.offset_deg === 'number') imuRosState.headingOffsetDeg = imuHeading.offset_deg;
|
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.raw_heading_deg === 'number') imuRosState.headingRawDeg = imuHeading.raw_heading_deg;
|
||||||
if (typeof imuHeading.corrected_heading_deg === 'number') imuRosState.headingCorrectedDeg = imuHeading.corrected_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');
|
setAdminStatus(data.status || 'Bereit');
|
||||||
setText('info_ips', system.ips);
|
setText('info_ips', system.ips);
|
||||||
setText('info_wifi', system.wifi);
|
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() {
|
async function setImuNorth() {
|
||||||
if (!window.confirm('Rover jetzt wirklich sauber nach Norden ausgerichtet?')) return;
|
if (!window.confirm('Rover jetzt wirklich sauber nach Norden ausgerichtet?')) return;
|
||||||
setAdminStatus('Speichere Nordrichtung...');
|
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() {
|
async function fetchDelay() {
|
||||||
try {
|
try {
|
||||||
var response = await fetch(adminApiBase + '/api/delay');
|
var response = await fetch(adminApiBase + '/api/delay');
|
||||||
|
|||||||
+19
-7
@@ -217,10 +217,15 @@ header {
|
|||||||
background: rgba(0,255,145,.07);
|
background: rgba(0,255,145,.07);
|
||||||
}
|
}
|
||||||
.dbtn.active {
|
.dbtn.active {
|
||||||
border-color: rgba(0,255,145,.7);
|
border-color: rgba(220,50,50,.75);
|
||||||
background: rgba(0,255,145,.1);
|
background: rgba(200,30,30,.12);
|
||||||
color: #00ff91;
|
color: #ff6060;
|
||||||
box-shadow: 0 0 20px rgba(0,255,145,.25), inset 0 0 14px rgba(0,255,145,.06);
|
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 {
|
@keyframes mode-flash {
|
||||||
0% { box-shadow: 0 0 0px rgba(0,255,145,0), background-color: var(--pri); }
|
0% { box-shadow: 0 0 0px rgba(0,255,145,0), background-color: var(--pri); }
|
||||||
@@ -281,10 +286,11 @@ header {
|
|||||||
top: 50%;
|
top: 50%;
|
||||||
left: 50%;
|
left: 50%;
|
||||||
transform: translate(-50%, -50%);
|
transform: translate(-50%, -50%);
|
||||||
width: calc(270px * var(--cam-scale));
|
width: calc(405px * var(--cam-scale));
|
||||||
height: calc(270px * var(--cam-scale));
|
height: calc(405px * var(--cam-scale));
|
||||||
z-index: 10;
|
z-index: 10;
|
||||||
pointer-events: none;
|
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-spin { to { transform: rotate(360deg); } }
|
||||||
@keyframes xh-pulse {
|
@keyframes xh-pulse {
|
||||||
@@ -543,7 +549,13 @@ body.page-connection-degraded .admin-shell{
|
|||||||
transition: all .15s;
|
transition: all .15s;
|
||||||
text-transform: uppercase;
|
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 {
|
.drive-inline {
|
||||||
display: flex;
|
display: flex;
|
||||||
|
|||||||
+98
-52
@@ -94,7 +94,7 @@
|
|||||||
S: <span id="overlay-distance">0,00</span> m
|
S: <span id="overlay-distance">0,00</span> m
|
||||||
</div>
|
</div>
|
||||||
<div class="c-xhair">
|
<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 -->
|
<!-- 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"/>
|
<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 -->
|
<!-- 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"/>
|
<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>
|
<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 -->
|
<!-- 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-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="7.5" text-anchor="middle">P +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>
|
</g>
|
||||||
<!-- center dot -->
|
<!-- center dot -->
|
||||||
<circle cx="0" cy="0" r="3" fill="rgba(0,255,145,.92)"/>
|
<circle cx="0" cy="0" r="3" fill="rgba(0,255,145,.92)"/>
|
||||||
@@ -167,7 +167,6 @@
|
|||||||
</svg>
|
</svg>
|
||||||
</div>
|
</div>
|
||||||
<div class="c-scanlines"></div>
|
<div class="c-scanlines"></div>
|
||||||
<div class="c-vignette"></div>
|
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
@@ -324,9 +323,31 @@ var imuOverlayState = {
|
|||||||
pitchDeg: 0,
|
pitchDeg: 0,
|
||||||
yawDeg: 0,
|
yawDeg: 0,
|
||||||
yawOffsetDeg: 0,
|
yawOffsetDeg: 0,
|
||||||
|
rollOffsetDeg: 0,
|
||||||
|
pitchOffsetDeg: 0,
|
||||||
lastUpdateAt: 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) {
|
function setText(id, text) {
|
||||||
var el = document.getElementById(id);
|
var el = document.getElementById(id);
|
||||||
if (el) {
|
if (el) {
|
||||||
@@ -355,7 +376,7 @@ function formatSignedNumber(value, digits) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
function headingToCompass(deg) {
|
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];
|
return dirs[Math.round(((deg % 360) + 360) % 360 / 22.5) % 16];
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -806,8 +827,10 @@ function setRosStatus(text) {
|
|||||||
setText("ros-state", text);
|
setText("ros-state", text);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
var autoRotateActive = false;
|
||||||
|
|
||||||
function controlsAreBlocked() {
|
function controlsAreBlocked() {
|
||||||
return pageConnection.state === "offline";
|
return pageConnection.state === "offline" || autoRotateActive;
|
||||||
}
|
}
|
||||||
|
|
||||||
function updateControlAvailability() {
|
function updateControlAvailability() {
|
||||||
@@ -992,12 +1015,19 @@ function updateServiceState(payload) {
|
|||||||
var system = payload.system || {};
|
var system = payload.system || {};
|
||||||
var services = payload.services || {};
|
var services = payload.services || {};
|
||||||
var imuHeading = payload.imu_heading || {};
|
var imuHeading = payload.imu_heading || {};
|
||||||
|
var imuTilt = payload.imu_tilt || {};
|
||||||
var cameraServiceState = String(services.camera || "");
|
var cameraServiceState = String(services.camera || "");
|
||||||
var cameraSettingsVersion = Number(services.camera_settings_version || 0);
|
var cameraSettingsVersion = Number(services.camera_settings_version || 0);
|
||||||
if (typeof imuHeading.offset_deg === "number" && !isNaN(imuHeading.offset_deg)) {
|
if (typeof imuHeading.offset_deg === "number" && !isNaN(imuHeading.offset_deg)) {
|
||||||
imuOverlayState.yawOffsetDeg = Number(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");
|
updateUndervoltageDisplay(system.undervoltage || "unbekannt", system.undervoltage_state || "unknown");
|
||||||
if (cameraServiceState === "active" && lastCameraServiceState && lastCameraServiceState !== "active") {
|
if (cameraServiceState === "active" && lastCameraServiceState && lastCameraServiceState !== "active") {
|
||||||
setText("cam-state", "Verbinde...");
|
setText("cam-state", "Verbinde...");
|
||||||
@@ -1132,12 +1162,14 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
|
|||||||
context.save();
|
context.save();
|
||||||
context.translate(cx, cy);
|
context.translate(cx, cy);
|
||||||
context.rotate(-rollRad);
|
context.rotate(-rollRad);
|
||||||
|
context.shadowColor = "rgba(0,0,0,.85)";
|
||||||
|
context.shadowBlur = 3;
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.moveTo(0, -90 * unit);
|
context.moveTo(0, -90 * unit);
|
||||||
context.lineTo(-5.5 * unit, -79 * unit);
|
context.lineTo(-5.5 * unit, -79 * unit);
|
||||||
context.lineTo(5.5 * unit, -79 * unit);
|
context.lineTo(5.5 * unit, -79 * unit);
|
||||||
context.closePath();
|
context.closePath();
|
||||||
context.fillStyle = "rgba(0,255,145,.88)";
|
context.fillStyle = hudRgba(.88);
|
||||||
context.fill();
|
context.fill();
|
||||||
context.restore();
|
context.restore();
|
||||||
|
|
||||||
@@ -1146,6 +1178,8 @@ function drawImuHorizonOverlay(context, canvasWidth, canvasHeight, size) {
|
|||||||
context.translate(cx, cy);
|
context.translate(cx, cy);
|
||||||
context.rotate(-rollRad);
|
context.rotate(-rollRad);
|
||||||
context.translate(0, pitchOffsetPx);
|
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 },
|
{ 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.beginPath();
|
||||||
context.moveTo(-pl.span * unit, pl.y * unit);
|
context.moveTo(-pl.span * unit, pl.y * unit);
|
||||||
context.lineTo(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.lineWidth = 0.9 * unit;
|
||||||
context.stroke();
|
context.stroke();
|
||||||
});
|
});
|
||||||
|
|
||||||
context.strokeStyle = "rgba(0,255,145,.95)";
|
context.strokeStyle = hudRgba(.95);
|
||||||
context.lineWidth = 2.8 * unit;
|
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(60 * unit, 0); context.stroke();
|
||||||
context.beginPath(); context.moveTo(-60 * unit, 0); context.lineTo(-52 * unit, -5 * unit); 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;
|
var bw = 84 * unit, bh = 15 * unit;
|
||||||
context.fillStyle = "rgba(0,15,8,.65)";
|
context.fillStyle = "rgba(0,15,8,.65)";
|
||||||
context.fillRect(cx - bw / 2, cy - 72 * unit, bw, bh);
|
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.font = (9 * unit) + 'px "Consolas","Cascadia Mono",monospace';
|
||||||
context.fillStyle = "rgba(0,255,145,.92)";
|
context.fillStyle = hudRgba(.92);
|
||||||
context.textAlign = "center";
|
context.textAlign = "center";
|
||||||
context.textBaseline = "middle";
|
context.textBaseline = "middle";
|
||||||
context.fillText("HDG " + String(Math.round(headingDeg)).padStart(3, "0") + "° " + headingToCompass(headingDeg), cx, cy - 62.5 * unit);
|
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.font = (10.5 * unit) + 'px "Consolas","Cascadia Mono",monospace';
|
||||||
context.fillStyle = "rgba(0,255,145,.72)";
|
context.fillStyle = hudRgba(.82);
|
||||||
context.fillText("R " + formatSignedNumber(rollDeg, 1) + "°", cx - 73 * unit, cy + 76 * unit);
|
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.fillText("P " + formatSignedNumber(pitchDeg, 1) + "°", cx + 73 * unit, cy + 76 * unit);
|
||||||
context.restore();
|
context.restore();
|
||||||
@@ -1189,67 +1225,76 @@ function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size)
|
|||||||
var cy = canvasHeight / 2;
|
var cy = canvasHeight / 2;
|
||||||
var unit = size / 180;
|
var unit = size / 180;
|
||||||
|
|
||||||
function circle(radius, stroke, lineWidth, dash) {
|
function circle(radius, alpha, lineWidth, dash) {
|
||||||
context.save();
|
context.save();
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.arc(cx, cy, radius * unit, 0, Math.PI * 2);
|
context.arc(cx, cy, radius * unit, 0, Math.PI * 2);
|
||||||
if (dash) context.setLineDash(dash.map(function (value) { return value * unit; }));
|
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.lineWidth = lineWidth * unit;
|
||||||
context.stroke();
|
context.stroke();
|
||||||
context.restore();
|
context.restore();
|
||||||
}
|
}
|
||||||
|
|
||||||
function line(x1, y1, x2, y2, stroke, lineWidth) {
|
function line(x1, y1, x2, y2, alpha, lineWidth) {
|
||||||
context.save();
|
context.save();
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.moveTo(cx + (x1 * unit), cy + (y1 * unit));
|
context.moveTo(cx + (x1 * unit), cy + (y1 * unit));
|
||||||
context.lineTo(cx + (x2 * unit), cy + (y2 * 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.lineWidth = lineWidth * unit;
|
||||||
context.stroke();
|
context.stroke();
|
||||||
context.restore();
|
context.restore();
|
||||||
}
|
}
|
||||||
|
|
||||||
function path(points, stroke, lineWidth) {
|
function path(points, alpha, lineWidth) {
|
||||||
context.save();
|
context.save();
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.moveTo(cx + (points[0][0] * unit), cy + (points[0][1] * unit));
|
context.moveTo(cx + (points[0][0] * unit), cy + (points[0][1] * unit));
|
||||||
for (var i = 1; i < points.length; i += 1) {
|
for (var i = 1; i < points.length; i += 1) {
|
||||||
context.lineTo(cx + (points[i][0] * unit), cy + (points[i][1] * unit));
|
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.lineWidth = lineWidth * unit;
|
||||||
context.stroke();
|
context.stroke();
|
||||||
context.restore();
|
context.restore();
|
||||||
}
|
}
|
||||||
|
|
||||||
circle(82, "rgba(0,255,145,.2)", 1, [4, 10]);
|
circle(82, .2, 1, [4, 10]);
|
||||||
circle(60, "rgba(0,255,145,.15)", 0.75, [2, 16]);
|
circle(60, .15, 0.75, [2, 16]);
|
||||||
circle(16, "rgba(0,255,145,.3)", 0.75);
|
circle(16, .3, 0.75);
|
||||||
line(-86, 0, -19, 0, "rgba(0,255,145,.6)", 1);
|
line(-86, 0, -19, 0, .6, 1);
|
||||||
line(19, 0, 86, 0, "rgba(0,255,145,.6)", 1);
|
line(19, 0, 86, 0, .6, 1);
|
||||||
line(0, -86, 0, -19, "rgba(0,255,145,.6)", 1);
|
line(0, -86, 0, -19, .6, 1);
|
||||||
line(0, 19, 0, 86, "rgba(0,255,145,.6)", 1);
|
line(0, 19, 0, 86, .6, 1);
|
||||||
line(-60, -6, -60, 6, "rgba(0,255,145,.45)", 0.75);
|
line(-60, -6, -60, 6, .45, 0.75);
|
||||||
line(60, -6, 60, 6, "rgba(0,255,145,.45)", 0.75);
|
line(60, -6, 60, 6, .45, 0.75);
|
||||||
line(-6, -60, 6, -60, "rgba(0,255,145,.45)", 0.75);
|
line(-6, -60, 6, -60, .45, 0.75);
|
||||||
line(-6, 60, 6, 60, "rgba(0,255,145,.45)", 0.75);
|
line(-6, 60, 6, 60, .45, 0.75);
|
||||||
line(-38, -3.5, -38, 3.5, "rgba(0,255,145,.3)", 0.5);
|
line(-38, -3.5, -38, 3.5, .3, 0.5);
|
||||||
line(38, -3.5, 38, 3.5, "rgba(0,255,145,.3)", 0.5);
|
line(38, -3.5, 38, 3.5, .3, 0.5);
|
||||||
line(-3.5, -38, 3.5, -38, "rgba(0,255,145,.3)", 0.5);
|
line(-3.5, -38, 3.5, -38, .3, 0.5);
|
||||||
line(-3.5, 38, 3.5, 38, "rgba(0,255,145,.3)", 0.5);
|
line(-3.5, 38, 3.5, 38, .3, 0.5);
|
||||||
path([[-74, -58], [-74, -74], [-58, -74]], "rgba(0,255,145,.75)", 1.5);
|
path([[-74, -58], [-74, -74], [-58, -74]], .75, 1.5);
|
||||||
path([[58, -74], [74, -74], [74, -58]], "rgba(0,255,145,.75)", 1.5);
|
path([[58, -74], [74, -74], [74, -58]], .75, 1.5);
|
||||||
path([[-74, 58], [-74, 74], [-58, 74]], "rgba(0,255,145,.75)", 1.5);
|
path([[-74, 58], [-74, 74], [-58, 74]], .75, 1.5);
|
||||||
path([[58, 74], [74, 74], [74, 58]], "rgba(0,255,145,.75)", 1.5);
|
path([[58, 74], [74, 74], [74, 58]], .75, 1.5);
|
||||||
circle(6, "rgba(0,255,145,.7)", 1);
|
circle(6, .7, 1);
|
||||||
|
|
||||||
context.save();
|
context.save();
|
||||||
|
context.shadowColor = "rgba(0,0,0,.85)";
|
||||||
|
context.shadowBlur = 3;
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.arc(cx, cy, 2.5 * unit, 0, Math.PI * 2);
|
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.fill();
|
||||||
|
context.shadowBlur = 0;
|
||||||
context.beginPath();
|
context.beginPath();
|
||||||
context.arc(cx, cy, 1 * unit, 0, Math.PI * 2);
|
context.arc(cx, cy, 1 * unit, 0, Math.PI * 2);
|
||||||
context.fillStyle = "rgba(255,255,255,.7)";
|
context.fillStyle = "rgba(255,255,255,.7)";
|
||||||
@@ -1262,17 +1307,9 @@ function drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, size)
|
|||||||
function drawVignetteAndScanlines(context, canvasWidth, canvasHeight) {
|
function drawVignetteAndScanlines(context, canvasWidth, canvasHeight) {
|
||||||
context.save();
|
context.save();
|
||||||
for (var y = 0; y < canvasHeight; y += 4) {
|
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);
|
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();
|
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.fillText(document.getElementById("cam-live-text").textContent, canvasWidth / 2 + (6 * camScale * scaleToCanvas), padding);
|
||||||
context.restore();
|
context.restore();
|
||||||
|
|
||||||
drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, 270 * camScale * scaleToCanvas);
|
drawCrosshairOverlayToCanvas(context, canvasWidth, canvasHeight, 405 * camScale * scaleToCanvas);
|
||||||
drawVignetteAndScanlines(context, canvasWidth, canvasHeight);
|
drawVignetteAndScanlines(context, canvasWidth, canvasHeight);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1535,7 +1572,16 @@ function toggleStatPanel() {
|
|||||||
setStatPanelCollapsed(!panel.classList.contains("collapsed"));
|
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 () {
|
window.addEventListener("load", function () {
|
||||||
|
try { var h = localStorage.getItem("hudHexColor"); if (h) applyHudColor(h); } catch (e) {}
|
||||||
setText("cam-host", hostUrl);
|
setText("cam-host", hostUrl);
|
||||||
updateModeDisplay();
|
updateModeDisplay();
|
||||||
updateMotorDisplay();
|
updateMotorDisplay();
|
||||||
@@ -1709,8 +1755,8 @@ window.addEventListener("load", function () {
|
|||||||
Number(orientation.z || 0),
|
Number(orientation.z || 0),
|
||||||
Number(orientation.w || 1)
|
Number(orientation.w || 1)
|
||||||
]);
|
]);
|
||||||
imuOverlayState.rollDeg = euler.rollDeg;
|
imuOverlayState.rollDeg = euler.rollDeg + imuOverlayState.rollOffsetDeg;
|
||||||
imuOverlayState.pitchDeg = euler.pitchDeg;
|
imuOverlayState.pitchDeg = euler.pitchDeg + imuOverlayState.pitchOffsetDeg;
|
||||||
imuOverlayState.yawDeg = euler.yawDeg;
|
imuOverlayState.yawDeg = euler.yawDeg;
|
||||||
imuOverlayState.lastUpdateAt = Date.now();
|
imuOverlayState.lastUpdateAt = Date.now();
|
||||||
updateImuOverlaySvg();
|
updateImuOverlaySvg();
|
||||||
|
|||||||
@@ -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
@@ -32,6 +32,9 @@ CAMERA_RUNTIME_SETTINGS_FILE = '/tmp/exomy_camera_settings.json'
|
|||||||
IMU_HEADING_CALIBRATION_FILE = os.path.join(
|
IMU_HEADING_CALIBRATION_FILE = os.path.join(
|
||||||
os.path.dirname(__file__), '..', 'config', 'imu_heading_calibration.json'
|
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()
|
MOTOR_TEST_LOCK = threading.Lock()
|
||||||
GPS_PORT_PATTERNS = [
|
GPS_PORT_PATTERNS = [
|
||||||
'/dev/serial/by-id/*',
|
'/dev/serial/by-id/*',
|
||||||
@@ -82,6 +85,7 @@ IMU_ROS_LOCK = threading.Lock()
|
|||||||
IMU_ROS_THREAD = None
|
IMU_ROS_THREAD = None
|
||||||
IMU_ROS_CLIENT = None
|
IMU_ROS_CLIENT = None
|
||||||
IMU_ROS_TOPICS = []
|
IMU_ROS_TOPICS = []
|
||||||
|
LAST_LOCOMOTION_MODE = 1 # ACKERMANN als Fallback
|
||||||
IMU_ROS_CACHE = {
|
IMU_ROS_CACHE = {
|
||||||
'state': 'unknown',
|
'state': 'unknown',
|
||||||
'status': 'Warte auf ROS',
|
'status': 'Warte auf ROS',
|
||||||
@@ -91,6 +95,8 @@ IMU_ROS_CACHE = {
|
|||||||
'magnetic': 'unbekannt',
|
'magnetic': 'unbekannt',
|
||||||
'quaternion': 'unbekannt',
|
'quaternion': 'unbekannt',
|
||||||
'roll_pitch_yaw': 'unbekannt',
|
'roll_pitch_yaw': 'unbekannt',
|
||||||
|
'roll_deg': None,
|
||||||
|
'pitch_deg': None,
|
||||||
'error': 'Noch keine IMU-Daten aus ROS',
|
'error': 'Noch keine IMU-Daten aus ROS',
|
||||||
'heading_deg': None,
|
'heading_deg': None,
|
||||||
'updated_at': 0.0,
|
'updated_at': 0.0,
|
||||||
@@ -195,6 +201,8 @@ def _on_imu_data_message(message):
|
|||||||
), 'rad/s', 3),
|
), 'rad/s', 3),
|
||||||
quaternion=_format_quaternion(quaternion),
|
quaternion=_format_quaternion(quaternion),
|
||||||
roll_pitch_yaw=_format_euler_deg(euler_deg),
|
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]),
|
heading_deg=None if not euler_deg else _normalize_heading_deg(euler_deg[2]),
|
||||||
error='',
|
error='',
|
||||||
)
|
)
|
||||||
@@ -220,6 +228,7 @@ def _connect_imu_ros():
|
|||||||
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
||||||
|
|
||||||
import roslibpy
|
import roslibpy
|
||||||
|
import imu_rotate
|
||||||
|
|
||||||
client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT)
|
client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT)
|
||||||
client.run()
|
client.run()
|
||||||
@@ -232,14 +241,26 @@ def _connect_imu_ros():
|
|||||||
imu_topic = roslibpy.Topic(client, '/imu/data', 'sensor_msgs/Imu')
|
imu_topic = roslibpy.Topic(client, '/imu/data', 'sensor_msgs/Imu')
|
||||||
mag_topic = roslibpy.Topic(client, '/imu/mag', 'sensor_msgs/MagneticField')
|
mag_topic = roslibpy.Topic(client, '/imu/mag', 'sensor_msgs/MagneticField')
|
||||||
status_topic = roslibpy.Topic(client, '/imu/status', 'std_msgs/String')
|
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)
|
imu_topic.subscribe(_on_imu_data_message)
|
||||||
mag_topic.subscribe(_on_imu_mag_message)
|
mag_topic.subscribe(_on_imu_mag_message)
|
||||||
status_topic.subscribe(_on_imu_status_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_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='')
|
_imu_ros_cache_update(status='ROS verbunden', error='')
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def _imu_ros_worker():
|
def _imu_ros_worker():
|
||||||
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
||||||
|
|
||||||
@@ -856,6 +877,7 @@ def collect_status():
|
|||||||
container_details = get_container_details(EXOMY_CONTAINER)
|
container_details = get_container_details(EXOMY_CONTAINER)
|
||||||
imu_details = read_imu_ros_status()
|
imu_details = read_imu_ros_status()
|
||||||
imu_heading = get_imu_heading_status()
|
imu_heading = get_imu_heading_status()
|
||||||
|
imu_tilt = get_imu_tilt_status()
|
||||||
return {
|
return {
|
||||||
'status': 'Bereit',
|
'status': 'Bereit',
|
||||||
'system': {
|
'system': {
|
||||||
@@ -899,6 +921,7 @@ def collect_status():
|
|||||||
},
|
},
|
||||||
'imu': imu_details,
|
'imu': imu_details,
|
||||||
'imu_heading': imu_heading,
|
'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):
|
def set_camera_config(profile_id=None, fps=None):
|
||||||
current = read_camera_settings()
|
current = read_camera_settings()
|
||||||
if profile_id is None:
|
if profile_id is None:
|
||||||
@@ -1869,6 +1971,20 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
|||||||
self.end_headers()
|
self.end_headers()
|
||||||
|
|
||||||
def do_GET(self):
|
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':
|
if self.path == '/api/delay':
|
||||||
self.write_json(200, {'delay_seconds': get_delay()})
|
self.write_json(200, {'delay_seconds': get_delay()})
|
||||||
return
|
return
|
||||||
@@ -1911,7 +2027,8 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
|||||||
|
|
||||||
def do_POST(self):
|
def do_POST(self):
|
||||||
raw_body = b'{}'
|
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:
|
try:
|
||||||
content_length = int(self.headers.get('Content-Length', '0'))
|
content_length = int(self.headers.get('Content-Length', '0'))
|
||||||
except ValueError:
|
except ValueError:
|
||||||
@@ -2197,6 +2314,43 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
|||||||
self.write_json(status_code, response)
|
self.write_json(status_code, response)
|
||||||
return
|
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 = {
|
actions = {
|
||||||
'/api/cold-start-gps': {
|
'/api/cold-start-gps': {
|
||||||
'handler': cold_start_gps_receiver,
|
'handler': cold_start_gps_receiver,
|
||||||
|
|||||||
@@ -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
@@ -141,24 +141,25 @@ def publish_measurements(imu_topic, mag_topic, status_topic, sensor):
|
|||||||
magnetic = sensor.magnetic
|
magnetic = sensor.magnetic
|
||||||
quaternion = sensor.quaternion
|
quaternion = sensor.quaternion
|
||||||
|
|
||||||
|
# Sensor ist um 90° um die Z-Achse verdreht eingebaut → X- und Y-Achse tauschen
|
||||||
imu_topic.publish(roslibpy.Message({
|
imu_topic.publish(roslibpy.Message({
|
||||||
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
|
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
|
||||||
'orientation': {
|
'orientation': {
|
||||||
'x': quaternion[0],
|
'x': quaternion[1],
|
||||||
'y': quaternion[1],
|
'y': quaternion[0],
|
||||||
'z': quaternion[2],
|
'z': quaternion[2],
|
||||||
'w': quaternion[3],
|
'w': quaternion[3],
|
||||||
},
|
},
|
||||||
'orientation_covariance': [0.0] * 9,
|
'orientation_covariance': [0.0] * 9,
|
||||||
'angular_velocity': {
|
'angular_velocity': {
|
||||||
'x': gyro[0],
|
'x': gyro[1],
|
||||||
'y': gyro[1],
|
'y': gyro[0],
|
||||||
'z': gyro[2],
|
'z': gyro[2],
|
||||||
},
|
},
|
||||||
'angular_velocity_covariance': [0.0] * 9,
|
'angular_velocity_covariance': [0.0] * 9,
|
||||||
'linear_acceleration': {
|
'linear_acceleration': {
|
||||||
'x': acceleration[0],
|
'x': acceleration[1],
|
||||||
'y': acceleration[1],
|
'y': acceleration[0],
|
||||||
'z': acceleration[2],
|
'z': acceleration[2],
|
||||||
},
|
},
|
||||||
'linear_acceleration_covariance': [0.0] * 9,
|
'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({
|
mag_topic.publish(roslibpy.Message({
|
||||||
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
|
'header': {'stamp': stamp, 'frame_id': FRAME_ID},
|
||||||
'magnetic_field': {
|
'magnetic_field': {
|
||||||
'x': magnetic[0] * 1e-6,
|
'x': magnetic[1] * 1e-6,
|
||||||
'y': magnetic[1] * 1e-6,
|
'y': magnetic[0] * 1e-6,
|
||||||
'z': magnetic[2] * 1e-6,
|
'z': magnetic[2] * 1e-6,
|
||||||
},
|
},
|
||||||
'magnetic_field_covariance': [0.0] * 9,
|
'magnetic_field_covariance': [0.0] * 9,
|
||||||
|
|||||||
@@ -180,6 +180,9 @@ def callback(data):
|
|||||||
global last_webgui_time
|
global last_webgui_time
|
||||||
global last_locomotion_mode
|
global last_locomotion_mode
|
||||||
|
|
||||||
|
if rospy.get_param('/auto_rotate_active', False):
|
||||||
|
return
|
||||||
|
|
||||||
is_webgui = data.header.frame_id == "webgui"
|
is_webgui = data.header.frame_id == "webgui"
|
||||||
now = rospy.Time.now()
|
now = rospy.Time.now()
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user