Compare commits
10
Commits
e7d7d2ae20
...
main
| 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.
|
||||
- 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
@@ -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
@@ -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
@@ -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();
|
||||
|
||||
@@ -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(
|
||||
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,
|
||||
|
||||
@@ -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
|
||||
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,
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user