Turn on Point kann jetzt schon im Admin aufgerufen werden um den Rover in eine Richtug zu drehen

2026-05-29 09:36:53 +02:00
parent 5e39a41f49
commit 4c24558516
3 changed files with 285 additions and 2 deletions
@@ -180,6 +180,24 @@
              <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="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">-</strong>
+          </div>
          <div class="sr">
            <span class="sr-led led-off"></span>
            <span class="sr-key">Hinweis</span>
@@ -2890,6 +2908,83 @@
    }
  }

+  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) {
+        txt += ' | Ist: ' + rotate.current_deg + '° | Rest: ' + rotate.error_deg + '°';
+      }
+      val.textContent = txt;
+    }
+    if (!running && rotatePoller) {
+      clearInterval(rotatePoller);
+      rotatePoller = null;
+    }
+  }
+
+  async function startRotate() {
+    var input = document.getElementById('rotate_target');
+    var target = parseFloat(input ? input.value : 0);
+    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})
+      });
+      var data = await response.json();
+      if (!response.ok) throw new Error(data.status || ('status ' + response.status));
+      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();
+      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');
@@ -227,6 +227,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()
@@ -239,13 +240,29 @@ 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')
    imu_topic.subscribe(_on_imu_data_message)
    mag_topic.subscribe(_on_imu_mag_message)
    status_topic.subscribe(_on_imu_status_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 _publish_rover_cmd(locomotion_mode, vel, steering):
+        try:
+            rover_cmd_topic.publish(roslibpy.Message({
+                'connected': True,
+                'motors_enabled': True,
+                'locomotion_mode': int(locomotion_mode),
+                'vel': int(vel),
+                'steering': int(steering),
+            }))
+        except Exception:
+            pass
+
+    imu_rotate.set_publish_fn(_publish_rover_cmd)
+

 def _imu_ros_worker():
    global IMU_ROS_CLIENT, IMU_ROS_TOPICS
@@ -1957,6 +1974,10 @@ class Handler(http.server.BaseHTTPRequestHandler):
        self.end_headers()

    def do_GET(self):
+        if self.path == '/api/imu/rotate-status':
+            import imu_rotate
+            self.write_json(200, {'rotate': imu_rotate.get_rotate_status()})
+            return
        if self.path == '/api/delay':
            self.write_json(200, {'delay_seconds': get_delay()})
            return
@@ -1999,7 +2020,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:
@@ -2290,6 +2312,39 @@ class Handler(http.server.BaseHTTPRequestHandler):
            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))
+            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']
+
+            def _get_corrected_heading():
+                with IMU_ROS_LOCK:
+                    raw = IMU_ROS_CACHE.get('heading_deg')
+                return get_corrected_heading_deg(raw, heading_offset) if raw is not None else None
+
+            status = imu_rotate.start_rotate(
+                target_deg=target_deg,
+                get_heading_fn=_get_corrected_heading,
+            )
+            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,133 @@
+"""
+Dreht den Rover auf der Stelle zu einem Ziel-Heading.
+
+Öffentliche API:
+    set_publish_fn(fn)   wird von exomy_admin_api nach ROS-Connect gesetzt
+    start_rotate(target_deg, get_heading_fn) -> dict
+    stop_rotate()
+    get_rotate_status() -> dict
+"""
+
+import threading
+import time
+
+_POINT_TURN = 2    # LocomotionMode.POINT_TURN
+_ACKERMANN  = 1    # LocomotionMode.ACKERMANN (Standard-Rückkehrmodus)
+_DONE_DEG   = 3.0  # Toleranz in Grad
+_VEL        = 20   # konstante Drehgeschwindigkeit (0–100), direkte Motoransteuerung
+_INTERVAL   = 0.02 # Sekunden zwischen Regelschritten (50 Hz)
+
+_lock       = threading.Lock()
+_thread     = None
+_stop_event = threading.Event()
+_publish_fn = None
+
+_status = {
+    'running':     False,
+    'target_deg':  None,
+    'current_deg': None,
+    'error_deg':   None,
+    'result':      'idle',
+    'message':     '',
+}
+
+
+def set_publish_fn(fn):
+    global _publish_fn
+    _publish_fn = fn
+
+
+def _publish(locomotion_mode, vel, steering):
+    fn = _publish_fn
+    if fn is not None:
+        fn(locomotion_mode=locomotion_mode, vel=vel, steering=steering)
+
+
+def _shortest_error(current, target):
+    """Vorzeichen: positiv = rechtsdrehung nötig, negativ = linksdrehung."""
+    return (target - current + 540) % 360 - 180
+
+
+def _restore(restore_mode):
+    _publish(restore_mode, vel=0, steering=0)
+
+
+def _control_loop(target_deg, get_heading_fn, restore_mode):
+    global _status
+
+    try:
+        while not _stop_event.is_set():
+            heading = get_heading_fn()
+            if heading is None:
+                time.sleep(0.05)
+                continue
+
+            error = _shortest_error(heading, target_deg)
+
+            with _lock:
+                _status['current_deg'] = round(heading, 1)
+                _status['error_deg']   = round(error, 1)
+
+            if abs(error) <= _DONE_DEG:
+                _restore(restore_mode)
+                with _lock:
+                    _status['running'] = False
+                    _status['result']  = 'done'
+                    _status['message'] = 'Ziel erreicht'
+                return
+
+            vel = _VEL if error > 0 else -_VEL
+            _publish(_POINT_TURN, vel=vel, steering=0)
+            time.sleep(_INTERVAL)
+
+        _restore(restore_mode)
+        with _lock:
+            _status['running'] = False
+            _status['result']  = 'aborted'
+            _status['message'] = 'Abgebrochen'
+
+    except Exception as exc:
+        _restore(restore_mode)
+        with _lock:
+            _status['running'] = False
+            _status['result']  = 'error'
+            _status['message'] = str(exc)
+
+
+def start_rotate(target_deg, get_heading_fn):
+    global _thread, _status
+
+    target_deg = float(target_deg) % 360
+    stop_rotate()
+
+    _stop_event.clear()
+    with _lock:
+        _status = {
+            'running':     True,
+            'target_deg':  round(target_deg, 1),
+            'current_deg': None,
+            'error_deg':   None,
+            'result':      'running',
+            'message':     'Drehe zu {0:.0f}°'.format(target_deg),
+        }
+
+    _thread = threading.Thread(
+        target=_control_loop,
+        args=(target_deg, get_heading_fn),
+        daemon=True,
+    )
+    _thread.start()
+    return get_rotate_status()
+
+
+def stop_rotate():
+    global _thread
+    if _thread is not None and _thread.is_alive():
+        _stop_event.set()
+        _thread.join(timeout=2.0)
+    _thread = None
+
+
+def get_rotate_status():
+    with _lock:
+        return dict(_status)