From cc44e05e57ea11b395d72619091ecf268d11c1dd Mon Sep 17 00:00:00 2001 From: Eskimue Date: Thu, 28 May 2026 15:30:33 +0200 Subject: [PATCH] admin verbessert --- gui/admin.html | 193 +++++++++++++++++++++++++++++++++++++++++++----- src/imu_node.py | 42 +++++++++-- 2 files changed, 210 insertions(+), 25 deletions(-) diff --git a/gui/admin.html b/gui/admin.html index af66f9a..67d7910 100644 --- a/gui/admin.html +++ b/gui/admin.html @@ -816,6 +816,9 @@ var roverCommandTopic = null; var motorCommandsTopic = null; var gpsFixTopic = null; + var imuDataTopic = null; + var imuMagTopic = null; + var imuStatusTopic = null; var gpsStatusTopic = null; var gpsDiagnosticsTopic = null; var pageConnection = { @@ -930,6 +933,20 @@ selectedSatellitePrn: null, plottedSatellites: [] }; + var imuRosState = { + state: 'unknown', + status: '-', + address: '0x4A', + acceleration: '--', + gyro: '--', + magnetic: '--', + quaternion: '--', + rollPitchYaw: '--', + error: '--', + lastDataAt: 0, + lastMagAt: 0, + lastStatusAt: 0 + }; function submitPassword() { var input = document.getElementById('auth_input'); @@ -982,14 +999,6 @@ setServiceState('camera_service_state', '-'); setServiceState('service_admin_api', '-'); setServiceState('service_exomy', '-'); - setStateText('imu_state', 'unknown', '-'); - setText('imu_address', '--'); - setText('imu_acceleration', '--'); - setText('imu_gyro', '--'); - setText('imu_magnetic', '--'); - setText('imu_quaternion', '--'); - setText('imu_roll_pitch_yaw', '--'); - setText('imu_error', '--'); var bar = document.getElementById('bar-temp-adm'); if (bar) bar.style.width = '0%'; } @@ -1032,6 +1041,19 @@ gpsDiagnosticsState.utcTime = null; gpsDiagnosticsState.lastFixAgeS = null; gpsDiagnosticsState.satellites = []; + imuRosState.state = 'unknown'; + imuRosState.status = 'ROS getrennt'; + imuRosState.address = '0x4A'; + imuRosState.acceleration = '--'; + imuRosState.gyro = '--'; + imuRosState.magnetic = '--'; + imuRosState.quaternion = '--'; + imuRosState.rollPitchYaw = '--'; + imuRosState.error = 'Keine ROS-Verbindung'; + imuRosState.lastDataAt = 0; + imuRosState.lastMagAt = 0; + imuRosState.lastStatusAt = 0; + renderImuRosState(); updateGpsDiagnosticsPanel(); } @@ -1213,6 +1235,81 @@ node.classList.add('state_unknown'); } + function formatImuVector(values, unit, decimals) { + if (!Array.isArray(values) || values.length < 3) return '--'; + var digits = typeof decimals === 'number' ? decimals : 2; + return 'X ' + Number(values[0]).toFixed(digits) + + ' | Y ' + Number(values[1]).toFixed(digits) + + ' | Z ' + Number(values[2]).toFixed(digits) + ' ' + unit; + } + + function formatImuQuaternion(values) { + if (!Array.isArray(values) || values.length < 4) return '--'; + return 'I ' + Number(values[0]).toFixed(3) + + ' | J ' + Number(values[1]).toFixed(3) + + ' | K ' + Number(values[2]).toFixed(3) + + ' | R ' + Number(values[3]).toFixed(3); + } + + function quaternionToEulerDeg(values) { + if (!Array.isArray(values) || values.length < 4) return null; + var x = Number(values[0]); + var y = Number(values[1]); + var z = Number(values[2]); + var w = Number(values[3]); + var sinrCosp = 2 * (w * x + y * z); + var cosrCosp = 1 - 2 * (x * x + y * y); + var roll = Math.atan2(sinrCosp, cosrCosp); + var sinp = 2 * (w * y - z * x); + var pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * (Math.PI / 2) : Math.asin(sinp); + var sinyCosp = 2 * (w * z + x * y); + var cosyCosp = 1 - 2 * (y * y + z * z); + var yaw = Math.atan2(sinyCosp, cosyCosp); + return [ + roll * 180 / Math.PI, + pitch * 180 / Math.PI, + yaw * 180 / Math.PI + ]; + } + + function formatRollPitchYaw(values) { + if (!Array.isArray(values) || values.length < 3) return '--'; + return 'Roll ' + Number(values[0]).toFixed(1) + '° | ' + + 'Pitch ' + Number(values[1]).toFixed(1) + '° | ' + + 'Yaw ' + Number(values[2]).toFixed(1) + '°'; + } + + function renderImuRosState() { + var now = Date.now(); + var dataFresh = imuRosState.lastDataAt && (now - imuRosState.lastDataAt) < 3000; + var magFresh = imuRosState.lastMagAt && (now - imuRosState.lastMagAt) < 3000; + var statusFresh = imuRosState.lastStatusAt && (now - imuRosState.lastStatusAt) < 5000; + var state = imuRosState.state; + var status = imuRosState.status; + var error = imuRosState.error; + + if (!pageConnection.rosOk) { + state = 'unknown'; + status = 'ROS getrennt'; + error = 'Keine ROS-Verbindung'; + } else if (!dataFresh) { + state = 'unknown'; + status = 'Warte auf IMU-Daten'; + if (!error || error === '--') error = 'Noch keine frischen IMU-Daten'; + } else if (!statusFresh && (!status || status === '-')) { + status = 'Verbunden'; + } + + setStateText('imu_state', state, status || '-'); + setText('imu_address', imuRosState.address); + setText('imu_acceleration', dataFresh ? imuRosState.acceleration : '--'); + setText('imu_gyro', dataFresh ? imuRosState.gyro : '--'); + setText('imu_magnetic', magFresh ? imuRosState.magnetic : '--'); + setText('imu_quaternion', dataFresh ? imuRosState.quaternion : '--'); + setText('imu_roll_pitch_yaw', dataFresh ? imuRosState.rollPitchYaw : '--'); + setText('imu_error', error || '-'); + } + function formatNumber(value, digits) { return Number(value || 0).toLocaleString('de-DE', { minimumFractionDigits: digits, @@ -2432,6 +2529,12 @@ ros = null; roverCommandTopic = null; motorCommandsTopic = null; + gpsFixTopic = null; + gpsStatusTopic = null; + gpsDiagnosticsTopic = null; + imuDataTopic = null; + imuMagTopic = null; + imuStatusTopic = null; window.setTimeout(connectDriveEstimatorRos, 3000); }); ros.on('error', function() { @@ -2482,6 +2585,69 @@ messageType: 'std_msgs/String' }); gpsDiagnosticsTopic.subscribe(handleGpsDiagnostics); + + imuDataTopic = new ROSLIB.Topic({ + ros: ros, + name: '/imu/data', + messageType: 'sensor_msgs/Imu' + }); + imuDataTopic.subscribe(function(message) { + var linearAcceleration = message.linear_acceleration || {}; + var angularVelocity = message.angular_velocity || {}; + var orientation = message.orientation || {}; + var quaternion = [ + Number(orientation.x || 0), + Number(orientation.y || 0), + Number(orientation.z || 0), + Number(orientation.w || 1) + ]; + imuRosState.state = 'active'; + imuRosState.address = '0x4A'; + imuRosState.acceleration = formatImuVector([ + Number(linearAcceleration.x || 0), + Number(linearAcceleration.y || 0), + Number(linearAcceleration.z || 0) + ], 'm/s^2', 2); + imuRosState.gyro = formatImuVector([ + Number(angularVelocity.x || 0), + Number(angularVelocity.y || 0), + Number(angularVelocity.z || 0) + ], 'rad/s', 3); + imuRosState.quaternion = formatImuQuaternion(quaternion); + imuRosState.rollPitchYaw = formatRollPitchYaw(quaternionToEulerDeg(quaternion)); + imuRosState.lastDataAt = Date.now(); + if (!imuRosState.error || imuRosState.error === 'Keine ROS-Verbindung' || imuRosState.error === 'Noch keine frischen IMU-Daten') { + imuRosState.error = ''; + } + }); + + imuMagTopic = new ROSLIB.Topic({ + ros: ros, + name: '/imu/mag', + messageType: 'sensor_msgs/MagneticField' + }); + imuMagTopic.subscribe(function(message) { + var magneticField = message.magnetic_field || {}; + imuRosState.magnetic = formatImuVector([ + Number(magneticField.x || 0) * 1000000, + Number(magneticField.y || 0) * 1000000, + Number(magneticField.z || 0) * 1000000 + ], 'uT', 2); + imuRosState.lastMagAt = Date.now(); + }); + + imuStatusTopic = new ROSLIB.Topic({ + ros: ros, + name: '/imu/status', + messageType: 'std_msgs/String' + }); + imuStatusTopic.subscribe(function(message) { + var text = String((message && message.data) || '').trim(); + imuRosState.status = text || 'Verbunden'; + imuRosState.state = text.indexOf('Fehler') === 0 ? 'unknown' : 'active'; + imuRosState.error = text.indexOf('Fehler') === 0 ? text : ''; + imuRosState.lastStatusAt = Date.now(); + }); } function temperatureToWidth(value) { @@ -2494,7 +2660,6 @@ function renderStatus(data) { var system = data.system || {}; var services = data.services || {}; - var imu = data.imu || {}; setAdminStatus(data.status || 'Bereit'); setText('info_ips', system.ips); setText('info_wifi', system.wifi); @@ -2530,14 +2695,6 @@ setServiceState('camera_service_state', services.camera); setServiceState('service_admin_api', services.admin_api); setServiceState('service_exomy', services.exomy); - setStateText('imu_state', imu.state || 'unknown', imu.status || '-'); - setText('imu_address', imu.address); - setText('imu_acceleration', imu.acceleration); - setText('imu_gyro', imu.gyro); - setText('imu_magnetic', imu.magnetic); - setText('imu_quaternion', imu.quaternion); - setText('imu_roll_pitch_yaw', imu.roll_pitch_yaw); - setText('imu_error', imu.error || '-'); } async function fetchStatus() { @@ -3109,6 +3266,7 @@ renderWaypointList(); applyWaypointPlanningState(); renderDriveEstimator(); + renderImuRosState(); updateGpsDiagnosticsPanel(); refreshCameraPreview(); window.setInterval(fetchStatus, 10000); @@ -3116,6 +3274,7 @@ window.setInterval(fetchDelay, 5000); window.setInterval(fetchSpeedLimit, 5000); window.setInterval(fetchMotorTestStatus, 4000); + window.setInterval(renderImuRosState, 750); window.setInterval(renderDriveEstimator, 250); window.setInterval(updatePageConnectionState, 1000); }); diff --git a/src/imu_node.py b/src/imu_node.py index 65e48ad..b09fb6b 100644 --- a/src/imu_node.py +++ b/src/imu_node.py @@ -26,6 +26,7 @@ ROSBRIDGE_PORT = int(os.environ.get('IMU_ROSBRIDGE_PORT', '9090')) RUNNING = True WORKAROUND_APPLIED = False +LAST_ERROR_TEXT = None def handle_shutdown(signum, frame): @@ -40,6 +41,7 @@ def apply_bno08x_workaround(): if WORKAROUND_APPLIED: return + adafruit_bno08x._dbg = lambda *args, **kwargs: None original_handle_packet = adafruit_bno08x.BNO08X._handle_packet def safe_handle_packet(self, packet): @@ -49,6 +51,10 @@ def apply_bno08x_workaround(): if len(exc.args) == 1 and isinstance(exc.args[0], int): return raise + except IndexError as exc: + if 'list assignment index out of range' in str(exc): + return + raise adafruit_bno08x.BNO08X._handle_packet = safe_handle_packet WORKAROUND_APPLIED = True @@ -107,6 +113,27 @@ def publish_status(topic, text): topic.publish(roslibpy.Message({'data': text})) +def report_error(status_topic, text): + global LAST_ERROR_TEXT + + if text == LAST_ERROR_TEXT: + return + + LAST_ERROR_TEXT = text + print(text) + sys.stdout.flush() + if status_topic is not None: + try: + publish_status(status_topic, text) + except Exception: + pass + + +def clear_error_state(): + global LAST_ERROR_TEXT + LAST_ERROR_TEXT = None + + def publish_measurements(imu_topic, mag_topic, status_topic, sensor): stamp = ros_time_now() acceleration = sensor.acceleration @@ -174,23 +201,22 @@ def main(): if sensor is None: sensor = create_sensor() + clear_error_state() publish_status(status_topic, 'BNO085 initialisiert auf {0}'.format(ADDRESS)) print('BNO085 initialisiert') sys.stdout.flush() publish_measurements(imu_topic, mag_topic, status_topic, sensor) + clear_error_state() time.sleep(sleep_seconds) except Exception as exc: message = 'IMU Fehler: {0}'.format(exc) - print(message) - sys.stdout.flush() - if status_topic is not None and ros_client is not None and ros_client.is_connected: - try: - publish_status(status_topic, message) - except Exception: - pass + report_error(status_topic if ros_client is not None and ros_client.is_connected else None, message) sensor = None - time.sleep(1.0) + if isinstance(exc, OSError) and getattr(exc, 'errno', None) == 5: + time.sleep(1.5) + else: + time.sleep(1.0) if ros_client is not None: try: