diff --git a/ExoMy_Software-master/gui/app.css b/ExoMy_Software-master/gui/app.css index 001cb83..1ee71b2 100644 --- a/ExoMy_Software-master/gui/app.css +++ b/ExoMy_Software-master/gui/app.css @@ -319,7 +319,68 @@ header { .c-tr { top: calc(13px * var(--cam-scale)); right: calc(13px * var(--cam-scale)); text-align: right; } .c-bl { bottom: calc(13px * var(--cam-scale)); left: calc(13px * var(--cam-scale)); } .c-br { bottom: calc(13px * var(--cam-scale)); right: calc(13px * var(--cam-scale)); text-align: right; } +.c-bm { + position: absolute; + left: 50%; + bottom: calc(13px * var(--cam-scale)); + transform: translateX(-50%); + display: flex; + align-items: center; + gap: calc(8px * var(--cam-scale)); + z-index: 10; + pointer-events: none; +} +.c-bm-box { + display: flex; + align-items: baseline; + gap: calc(5px * var(--cam-scale)); + padding: calc(6px * var(--cam-scale)) calc(10px * var(--cam-scale)); + background: rgba(3, 10, 14, .72); + border: 1px solid rgba(0,255,145,.18); + box-shadow: inset 0 0 12px rgba(0,255,145,.04), 0 0 16px rgba(0,0,0,.18); + backdrop-filter: blur(2px); +} +.c-bm-lbl, +.c-bm-unit { + font-size: calc(8px * var(--cam-scale)); + color: rgba(0,255,145,.72); + letter-spacing: calc(1.2px * var(--cam-scale)); + text-transform: uppercase; +} +.c-bm-box strong { + font-size: calc(14px * var(--cam-scale)); + line-height: 1; + color: rgba(245,255,250,.98); + letter-spacing: calc(1px * var(--cam-scale)); +} /* ── STAT PANEL ─────────────────────────────────────── */ +.c-track { + position: absolute; + inset: 0; + width: 100%; + height: 100%; + z-index: 9; + pointer-events: none; + opacity: .92; + transition: opacity .18s ease; + filter: drop-shadow(0 0 8px rgba(0,255,145,.18)); +} +.track-edge, +.track-center { + fill: none; + stroke-linecap: round; + stroke-linejoin: round; +} +.track-edge { + stroke: rgba(170,255,225,.88); + stroke-width: 1.35; + stroke-dasharray: 6 5; +} +.track-center { + stroke: rgba(0,255,145,.38); + stroke-width: .9; + stroke-dasharray: 3 6; +} .stat-list { display: flex; flex-direction: column; diff --git a/ExoMy_Software-master/gui/index.html b/ExoMy_Software-master/gui/index.html index a022098..628ec47 100644 --- a/ExoMy_Software-master/gui/index.html +++ b/ExoMy_Software-master/gui/index.html @@ -83,6 +83,11 @@ m +
@@ -274,6 +279,7 @@ var hostUrl = window.location.hostname; var ros = null; var joyListener = null; var roverCommandListener = null; +var motorCommandsListener = null; var publishTimer = null; var statusTimer = null; var axes = [0, 0, 0, 0, 0, 0]; @@ -283,6 +289,23 @@ var motorsEnabled = true; var inputMode = "joystick"; var metBase = Date.now(); var dragging = false; +var REQUIRED_CALIBRATION_POINTS = [0, 20, 30, 40, 50, 60, 80, 100]; +var driveEstimator = { + drivePwmNeutralAverage: 307, + drivePwmRange: 100, + pwmFrequencyHz: 50, + speedMps: 0, + distanceM: 0, + lastTickMs: Date.now(), + motorsEnabled: false, + locomotionMode: 1, + motorSpeeds: [0, 0, 0, 0, 0, 0], + calibrationPoints: [] +}; +var trajectoryOverlay = { + steering: 90, + velocity: 0 +}; function setText(id, text) { var el = document.getElementById(id); @@ -291,6 +314,241 @@ function setText(id, text) { } } +function formatNumber(value, digits) { + return Number(value || 0).toLocaleString("de-DE", { + minimumFractionDigits: digits, + maximumFractionDigits: digits + }); +} + +function formatSpeedCms(speedMps, digits) { + return formatNumber(Number(speedMps || 0) * 100, digits); +} + +function cloneCalibrationPoints(points) { + return (points || []).map(function (item) { + return { + percent: Number(item.percent), + speed_mps: Number(item.speed_mps) + }; + }); +} + +function normalizeCalibrationPoints(points) { + var source = cloneCalibrationPoints(points).sort(function (a, b) { + return a.percent - b.percent; + }); + if (!source.length) { + source = [{ percent: 0, speed_mps: 0 }]; + } + + return REQUIRED_CALIBRATION_POINTS.map(function (targetPercent) { + var exact = source.find(function (point) { + return Number(point.percent) === Number(targetPercent); + }); + if (exact) { + return { + percent: targetPercent, + speed_mps: Number(exact.speed_mps || 0) + }; + } + + var lower = null; + var upper = null; + source.forEach(function (point) { + if (point.percent < targetPercent) lower = point; + if (!upper && point.percent > targetPercent) upper = point; + }); + + if (!lower && upper) { + return { + percent: targetPercent, + speed_mps: Number(upper.speed_mps || 0) + }; + } + if (lower && !upper) { + return { + percent: targetPercent, + speed_mps: Number(lower.speed_mps || 0) + }; + } + if (lower && upper) { + var span = Number(upper.percent) - Number(lower.percent); + var ratio = span <= 0 ? 0 : (targetPercent - Number(lower.percent)) / span; + return { + percent: targetPercent, + speed_mps: Number(lower.speed_mps || 0) + (Number(upper.speed_mps || 0) - Number(lower.speed_mps || 0)) * ratio + }; + } + + return { + percent: targetPercent, + speed_mps: 0 + }; + }); +} + +function interpolateCalibrationSpeed(percentMagnitude) { + var points = driveEstimator.calibrationPoints; + if (!points || !points.length) return 0; + var p = Math.max(0, Math.min(100, Math.abs(percentMagnitude))); + if (p <= points[0].percent) return Number(points[0].speed_mps || 0); + for (var i = 1; i < points.length; i += 1) { + var left = points[i - 1]; + var right = points[i]; + if (p <= right.percent) { + var span = right.percent - left.percent; + if (span <= 0) return Number(right.speed_mps || 0); + var ratio = (p - left.percent) / span; + return Number(left.speed_mps || 0) + (Number(right.speed_mps || 0) - Number(left.speed_mps || 0)) * ratio; + } + } + return Number(points[points.length - 1].speed_mps || 0); +} + +function getAverageMotorSpeedPercent() { + if (!driveEstimator.motorSpeeds || !driveEstimator.motorSpeeds.length) return 0; + var sum = 0; + var count = 0; + driveEstimator.motorSpeeds.forEach(function (value) { + if (typeof value === "number" && !isNaN(value)) { + sum += value; + count += 1; + } + }); + return count ? (sum / count) : 0; +} + +function isTranslationMode() { + return driveEstimator.locomotionMode !== 2; +} + +function updateDriveEstimatorModel() { + var nowMs = Date.now(); + var dt = Math.max(0, (nowMs - driveEstimator.lastTickMs) / 1000.0); + driveEstimator.lastTickMs = nowMs; + + var avgSpeedPercent = getAverageMotorSpeedPercent(); + var direction = avgSpeedPercent > 0 ? 1 : (avgSpeedPercent < 0 ? -1 : 0); + var speedMagnitude = isTranslationMode() && driveEstimator.motorsEnabled + ? interpolateCalibrationSpeed(avgSpeedPercent) + : 0; + var speedMps = direction === 0 ? 0 : speedMagnitude * direction; + + if (Math.abs(speedMps) < 0.0005) speedMps = 0; + if (dt > 0 && dt < 2.5) driveEstimator.distanceM += Math.abs(speedMps) * dt; + driveEstimator.speedMps = speedMps; +} + +function renderDriveEstimatorOverlay() { + updateDriveEstimatorModel(); + setText("overlay-speed", formatSpeedCms(driveEstimator.speedMps, 1)); + setText("overlay-distance", formatNumber(driveEstimator.distanceM, 2)); +} + +function setPath(id, d) { + var node = document.getElementById(id); + if (node) node.setAttribute("d", d || ""); +} + +function renderTrajectoryOverlay() { + var overlay = document.getElementById("track-overlay"); + if (!overlay) return; + + var moving = driveEstimator.motorsEnabled && Math.abs(driveEstimator.speedMps) > 0.005; + overlay.style.opacity = moving ? "1" : "0.22"; + var baseX = 50; + var baseY = 92; + var laneHalf = 9; + var topY = 18; + var leftStart = baseX - laneHalf; + var rightStart = baseX + laneHalf; + + if (!moving) { + setPath("track-center", "M " + baseX + " " + baseY + " L " + baseX + " " + topY); + setPath("track-left", "M " + leftStart + " " + baseY + " L " + leftStart + " " + topY); + setPath("track-right", "M " + rightStart + " " + baseY + " L " + rightStart + " " + topY); + return; + } + + var mode = driveEstimator.locomotionMode; + var steering = Number(trajectoryOverlay.steering || 90); + var direction = driveEstimator.speedMps < -0.005 ? -1 : 1; + + if (mode === 3) { + var crabOffset = Math.max(-24, Math.min(24, (steering >= 0 ? steering - 90 : steering + 90) * 0.26)); + setPath("track-center", "M " + baseX + " " + baseY + " L " + (baseX + crabOffset) + " " + topY); + setPath("track-left", "M " + (baseX - laneHalf) + " " + baseY + " L " + (baseX - laneHalf + crabOffset) + " " + topY); + setPath("track-right", "M " + (baseX + laneHalf) + " " + baseY + " L " + (baseX + laneHalf + crabOffset) + " " + topY); + return; + } + + if (mode === 2) { + var pointTurnRight = steering > 95 || steering < -95; + var sweep = pointTurnRight ? 1 : 0; + var radiusOuter = 34; + var radiusInner = 20; + var centerX = pointTurnRight ? 78 : 22; + var startLeftX = pointTurnRight ? 40 : 24; + var startRightX = pointTurnRight ? 58 : 42; + setPath("track-center", "M " + baseX + " " + baseY + " A 27 27 0 0 " + sweep + " " + centerX + " 42"); + setPath("track-left", "M " + startLeftX + " " + baseY + " A " + radiusOuter + " " + radiusOuter + " 0 0 " + sweep + " " + centerX + " 18"); + setPath("track-right", "M " + startRightX + " " + baseY + " A " + radiusInner + " " + radiusInner + " 0 0 " + sweep + " " + centerX + " 34"); + return; + } + + var normalizedTurn; + if (steering > 90) { + normalizedTurn = Math.min(1, (steering - 90) / 90); + } else { + normalizedTurn = -Math.min(1, (90 - steering) / 90); + } + normalizedTurn *= direction; + + var endX = baseX + normalizedTurn * 26; + var controlY = 58; + var controlBias = normalizedTurn * 18; + var centerPath = "M " + baseX + " " + baseY + " Q " + (baseX + controlBias * 0.6) + " " + controlY + " " + endX + " " + topY; + var leftPath = "M " + leftStart + " " + baseY + " Q " + (leftStart + controlBias) + " " + controlY + " " + (endX - laneHalf) + " " + topY; + var rightPath = "M " + rightStart + " " + baseY + " Q " + (rightStart + controlBias) + " " + controlY + " " + (endX + laneHalf) + " " + topY; + + if (Math.abs(normalizedTurn) < 0.04) { + centerPath = "M " + baseX + " " + baseY + " L " + baseX + " " + topY; + leftPath = "M " + leftStart + " " + baseY + " L " + leftStart + " " + topY; + rightPath = "M " + rightStart + " " + baseY + " L " + rightStart + " " + topY; + } + + setPath("track-center", centerPath); + setPath("track-left", leftPath); + setPath("track-right", rightPath); +} + +function fetchDriveEstimatorData() { + fetch(window.location.protocol + "//" + hostUrl + ":8082/api/drive-estimator/config", { + cache: "no-store" + }).then(function (response) { + if (!response.ok) throw new Error("config"); + return response.json(); + }).then(function (payload) { + var estimator = payload.estimator || {}; + driveEstimator.drivePwmNeutralAverage = Number(estimator.drive_pwm_neutral_average || driveEstimator.drivePwmNeutralAverage); + driveEstimator.drivePwmRange = Number(estimator.drive_pwm_range || driveEstimator.drivePwmRange); + driveEstimator.pwmFrequencyHz = Number(estimator.pwm_frequency_hz || driveEstimator.pwmFrequencyHz); + }).catch(function () {}); + + fetch(window.location.protocol + "//" + hostUrl + ":8082/api/drive-estimator/calibration", { + cache: "no-store" + }).then(function (response) { + if (!response.ok) throw new Error("calibration"); + return response.json(); + }).then(function (payload) { + var calibration = payload.calibration || {}; + var points = Array.isArray(calibration.points) ? calibration.points : []; + driveEstimator.calibrationPoints = normalizeCalibrationPoints(points); + renderDriveEstimatorOverlay(); + }).catch(function () {}); +} + function formatAxis(value) { return (value >= 0 ? "+" : "") + value.toFixed(2); } @@ -677,7 +935,12 @@ window.addEventListener("load", function () { updateClocks(); window.setInterval(updateClocks, 1000); fetchSystemStatus(); + fetchDriveEstimatorData(); + renderDriveEstimatorOverlay(); + renderTrajectoryOverlay(); statusTimer = window.setInterval(fetchSystemStatus, 5000); + window.setInterval(renderDriveEstimatorOverlay, 250); + window.setInterval(renderTrajectoryOverlay, 250); var video = document.getElementById("video_output"); video.src = "//" + hostUrl + ":8083/stream.mjpg"; @@ -745,7 +1008,24 @@ window.addEventListener("load", function () { roverCommandListener.subscribe(function (message) { applyDriveMode(locomotionModeToUi(message.locomotion_mode), true); motorsEnabled = !!message.motors_enabled; + driveEstimator.motorsEnabled = !!message.motors_enabled; + driveEstimator.locomotionMode = Number(message.locomotion_mode); + trajectoryOverlay.steering = Number(message.steering || 90); + trajectoryOverlay.velocity = Number(message.vel || 0); updateMotorDisplay(); + renderDriveEstimatorOverlay(); + renderTrajectoryOverlay(); + }); + + motorCommandsListener = new ROSLIB.Topic({ + ros: ros, + name: "/motor_commands", + messageType: "exomy/MotorCommands" + }); + motorCommandsListener.subscribe(function (message) { + driveEstimator.motorSpeeds = Array.isArray(message.motor_speeds) ? message.motor_speeds.map(Number) : [0, 0, 0, 0, 0, 0]; + renderDriveEstimatorOverlay(); + renderTrajectoryOverlay(); }); var joySubscriber = new ROSLIB.Topic({