3392 lines
113 KiB
JavaScript
3392 lines
113 KiB
JavaScript
const el = (id) => document.getElementById(id);
|
||
|
||
const statusEl = el("status");
|
||
const listEl = el("lidarList");
|
||
const lidarFormHintEl = el("lidarFormHint");
|
||
const pageTitleEl = document.querySelector(".pageTitle");
|
||
const navItemEls = Array.from(document.querySelectorAll(".navItem[data-page]"));
|
||
const pageOverviewEl = el("pageOverview");
|
||
const pageConfigEl = el("pageConfig");
|
||
const overviewBackendEl = el("overviewBackend");
|
||
const overviewActiveLayoutEl = el("overviewActiveLayout");
|
||
const overviewActiveModelEl = el("overviewActiveModel");
|
||
const overviewActiveSensorsEl = el("overviewActiveSensors");
|
||
const configSplitterEl = el("configSplitter");
|
||
|
||
const canvasWrap = el("canvasWrap");
|
||
const robotModelEl = el("robotModel");
|
||
const diffParamsEl = el("diffParams");
|
||
const bicycleParamsEl = el("bicycleParams");
|
||
const bicycleWheelbaseMEl = el("bicycleWheelbaseM");
|
||
const bicycleWheelRadiusMEl = el("bicycleWheelRadiusM");
|
||
const bicycleScaleMPerPxEl = el("bicycleScaleMPerPx");
|
||
const bicycleSteerPreviewDegEl = el("bicycleSteerPreviewDeg");
|
||
const bicycleSteerMaxDegEl = el("bicycleSteerMaxDeg");
|
||
const bicycleCmdVelTimeoutEl = el("bicycleCmdVelTimeout");
|
||
const bicycleLinearMaxVelEl = el("bicycleLinearMaxVel");
|
||
const bicycleLinearMaxAccelEl = el("bicycleLinearMaxAccel");
|
||
const bicycleValidationEl = el("bicycleValidation");
|
||
const bicycleMotorWheelsEl = el("bicycleMotorWheels");
|
||
const wheelSeparationMEl = el("wheelSeparationM");
|
||
const wheelRadiusMEl = el("wheelRadiusM");
|
||
const scaleMPerPxEl = el("scaleMPerPx");
|
||
const wheelSeparationMultEl = el("wheelSeparationMult");
|
||
const wheelRadiusMultEl = el("wheelRadiusMult");
|
||
const cmdVelTimeoutEl = el("cmdVelTimeout");
|
||
const linearMaxVelEl = el("linearMaxVel");
|
||
const linearMinVelEl = el("linearMinVel");
|
||
const linearMaxAccelEl = el("linearMaxAccel");
|
||
const angularMaxVelEl = el("angularMaxVel");
|
||
const angularMaxAccelEl = el("angularMaxAccel");
|
||
const diffValidationEl = el("diffValidation");
|
||
const robotDiffSummaryEl = el("robotDiffSummary");
|
||
const editFootprintBtn = el("editFootprintBtn");
|
||
const footprintEditHint = el("footprintEditHint");
|
||
const footprintShapeEl = el("footprintShape");
|
||
const footprintPresetPanelEl = el("footprintPresetPanel");
|
||
const fpRectParamsEl = el("fpRectParams");
|
||
const fpCircleParamsEl = el("fpCircleParams");
|
||
const fpPolyParamsEl = el("fpPolyParams");
|
||
const fpLengthMEl = el("fpLengthM");
|
||
const fpWidthMEl = el("fpWidthM");
|
||
const fpRadiusMEl = el("fpRadiusM");
|
||
const fpCircleSegmentsEl = el("fpCircleSegments");
|
||
const fpPolyRadiusMEl = el("fpPolyRadiusM");
|
||
const fpPolySidesEl = el("fpPolySides");
|
||
const applyFootprintPresetBtn = el("applyFootprintPresetBtn");
|
||
const footprintCustomPanelEl = el("footprintCustomPanel");
|
||
const fpVertexCountEl = el("fpVertexCount");
|
||
const fpSelectedVertexTextEl = el("fpSelectedVertexText");
|
||
const fpAddVertexBtn = el("fpAddVertexBtn");
|
||
const fpRemoveVertexBtn = el("fpRemoveVertexBtn");
|
||
const saveLayoutBtn = el("saveLayoutBtn");
|
||
const layoutSelectEl = el("layoutSelect");
|
||
const layoutNewNameEl = el("layoutNewName");
|
||
const layoutCloneCurrentEl = el("layoutCloneCurrent");
|
||
const layoutCreateBtn = el("layoutCreateBtn");
|
||
const layoutDeleteBtn = el("layoutDeleteBtn");
|
||
const layoutActiveHintEl = el("layoutActiveHint");
|
||
const lidarListCard = el("lidarListCard");
|
||
const lidarListCardToggle = el("lidarListCardToggle");
|
||
const imuListEl = el("imuList");
|
||
const imuListCard = el("imuListCard");
|
||
const imuListCardToggle = el("imuListCardToggle");
|
||
const imuFormHintEl = el("imuFormHint");
|
||
const robotModelCard = el("robotModelCard");
|
||
const robotModelCardToggle = el("robotModelCardToggle");
|
||
|
||
const canvas = el("canvas");
|
||
const ctx = canvas.getContext("2d");
|
||
|
||
const robotCenterText = el("robotCenterText");
|
||
const selectedText = el("selectedText");
|
||
const selectedRelText = el("selectedRelText");
|
||
|
||
const state = {
|
||
lidars: [],
|
||
imus: [],
|
||
layout: {
|
||
map: { width: 800, height: 600 },
|
||
robot: { x: 400, y: 300, yaw_deg: 0 },
|
||
// LiDAR pose is stored in ROBOT FRAME (ROS REP-103): x forward, y left, theta CCW around +Z.
|
||
// It is converted to canvas/world only for drawing and hit-testing.
|
||
lidarPoses: {}, // id -> {x,y,theta_deg}
|
||
lidarPosesFrame: "robot",
|
||
imuPoses: {}, // id -> {x,y,z,yaw_deg}
|
||
imuPosesFrame: "robot",
|
||
},
|
||
dragging: null, // {kind:'lidar'|'imu', id, dx, dy}
|
||
draggingFootprint: null, // {index, dx, dy}
|
||
selectedId: null,
|
||
selectedImuId: null,
|
||
selectedFootprintVertex: null,
|
||
editFootprint: false,
|
||
lidarListPanelCollapsed: false,
|
||
robotModelPanelCollapsed: false,
|
||
activeLayoutId: null,
|
||
activeLayoutName: "",
|
||
layoutCatalog: [],
|
||
layoutDirty: false,
|
||
lidarItemCollapsed: {}, // id -> true if collapsed
|
||
imuItemCollapsed: {},
|
||
imuListPanelCollapsed: false,
|
||
viewInitialized: false,
|
||
view: { scale: 1, panX: 0, panY: 0 },
|
||
panning: null, // { startSx, startSy, startPanX, startPanY }
|
||
pendingFootprintClick: null, // { sx, sy } when Shift+click may add a vertex
|
||
};
|
||
|
||
function setActivePage(page) {
|
||
const p = page === "overview" ? "overview" : "config";
|
||
navItemEls.forEach((a) => {
|
||
const on = (a.dataset.page || "") === p;
|
||
a.classList.toggle("active", on);
|
||
if (on) a.setAttribute("aria-current", "page");
|
||
else a.removeAttribute("aria-current");
|
||
});
|
||
if (pageTitleEl) pageTitleEl.textContent = p === "overview" ? "Tổng quan" : "Cấu Hình";
|
||
if (pageOverviewEl) pageOverviewEl.hidden = p !== "overview";
|
||
if (pageConfigEl) pageConfigEl.hidden = p !== "config";
|
||
try {
|
||
localStorage.setItem("activePage", p);
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
}
|
||
|
||
function initNavigation() {
|
||
navItemEls.forEach((a) => {
|
||
a.addEventListener("click", (evt) => {
|
||
evt.preventDefault();
|
||
setActivePage(a.dataset.page || "config");
|
||
});
|
||
});
|
||
// Restore last page, default to config (màn hình chính).
|
||
let initial = "config";
|
||
try {
|
||
const saved = localStorage.getItem("activePage");
|
||
if (saved === "overview" || saved === "config") initial = saved;
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
setActivePage(initial);
|
||
}
|
||
|
||
function setLeftPaneWidth(px) {
|
||
const v = Math.round(clamp(Number(px), 320, 720));
|
||
document.documentElement.style.setProperty("--leftPaneW", `${v}px`);
|
||
try {
|
||
localStorage.setItem("leftPaneW", String(v));
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
}
|
||
|
||
function initSplitPane() {
|
||
if (!configSplitterEl) return;
|
||
|
||
try {
|
||
const saved = Number(localStorage.getItem("leftPaneW"));
|
||
if (Number.isFinite(saved) && saved > 0) setLeftPaneWidth(saved);
|
||
else setLeftPaneWidth(460);
|
||
} catch {
|
||
setLeftPaneWidth(460);
|
||
}
|
||
|
||
let dragging = false;
|
||
let startX = 0;
|
||
let startW = 0;
|
||
|
||
const onMove = (evt) => {
|
||
if (!dragging) return;
|
||
const x = evt.clientX ?? (evt.touches && evt.touches[0] ? evt.touches[0].clientX : startX);
|
||
setLeftPaneWidth(startW + (x - startX));
|
||
};
|
||
const onUp = () => {
|
||
if (!dragging) return;
|
||
dragging = false;
|
||
configSplitterEl.classList.remove("dragging");
|
||
window.removeEventListener("mousemove", onMove);
|
||
window.removeEventListener("mouseup", onUp);
|
||
window.removeEventListener("touchmove", onMove);
|
||
window.removeEventListener("touchend", onUp);
|
||
};
|
||
|
||
configSplitterEl.addEventListener("mousedown", (evt) => {
|
||
evt.preventDefault();
|
||
dragging = true;
|
||
configSplitterEl.classList.add("dragging");
|
||
startX = evt.clientX;
|
||
startW = Number.parseFloat(getComputedStyle(document.documentElement).getPropertyValue("--leftPaneW")) || 460;
|
||
window.addEventListener("mousemove", onMove);
|
||
window.addEventListener("mouseup", onUp);
|
||
});
|
||
|
||
configSplitterEl.addEventListener("touchstart", (evt) => {
|
||
if (!evt.touches || !evt.touches[0]) return;
|
||
dragging = true;
|
||
configSplitterEl.classList.add("dragging");
|
||
startX = evt.touches[0].clientX;
|
||
startW = Number.parseFloat(getComputedStyle(document.documentElement).getPropertyValue("--leftPaneW")) || 460;
|
||
window.addEventListener("touchmove", onMove, { passive: true });
|
||
window.addEventListener("touchend", onUp);
|
||
}, { passive: false });
|
||
|
||
// Keyboard resize (focus splitter, use arrows)
|
||
configSplitterEl.addEventListener("keydown", (evt) => {
|
||
if (evt.key !== "ArrowLeft" && evt.key !== "ArrowRight") return;
|
||
evt.preventDefault();
|
||
const cur = Number.parseFloat(getComputedStyle(document.documentElement).getPropertyValue("--leftPaneW")) || 460;
|
||
setLeftPaneWidth(cur + (evt.key === "ArrowLeft" ? -20 : 20));
|
||
});
|
||
}
|
||
|
||
const DIFF_DEFAULTS = {
|
||
frame_id: "base_footprint",
|
||
wheel_separation_m: 1.0,
|
||
wheel_radius_m: 0.3,
|
||
wheel_separation_multiplier: 1.0,
|
||
wheel_radius_multiplier: 1.0,
|
||
scale_m_per_px: 0.005,
|
||
cmd_vel_timeout_s: 0.25,
|
||
linear: {
|
||
max_velocity: 1.0,
|
||
min_velocity: -0.5,
|
||
max_acceleration: 0.8,
|
||
min_acceleration: -0.4,
|
||
},
|
||
angular: {
|
||
max_velocity: 1.7,
|
||
max_acceleration: 1.5,
|
||
},
|
||
};
|
||
|
||
const BICYCLE_DEFAULTS = {
|
||
frame_id: "base_footprint",
|
||
wheelbase_m: 1.2,
|
||
wheel_radius_m: 0.15,
|
||
scale_m_per_px: 0.005,
|
||
steer_max_deg: 35,
|
||
steer_preview_deg: 15,
|
||
cmd_vel_timeout_s: 0.25,
|
||
linear_max_velocity: 1.0,
|
||
linear_max_acceleration: 0.8,
|
||
};
|
||
|
||
const DEFAULT_BICYCLE_WHEELS = {
|
||
rear: {
|
||
id: "rear",
|
||
role: "drive",
|
||
x_m: 0,
|
||
y_m: 0,
|
||
joint_name: "rear_wheel_joint",
|
||
motor: { vendor: "moons", model: "m2dc10a", gear_ratio: 20, invert: false },
|
||
},
|
||
front: {
|
||
id: "front",
|
||
role: "steer",
|
||
x_m: 1.2,
|
||
y_m: 0,
|
||
joint_name: "front_steer_joint",
|
||
motor: { vendor: "moons", model: "m2dc10a", gear_ratio: 20, invert: false },
|
||
},
|
||
};
|
||
|
||
const DEFAULT_WHEEL_MOTORS = {
|
||
left: {
|
||
id: "left",
|
||
side: "left",
|
||
joint_name: "wheel_left_joint",
|
||
motor: { vendor: "moons", model: "m2dc10a", gear_ratio: 20, invert: false },
|
||
},
|
||
right: {
|
||
id: "right",
|
||
side: "right",
|
||
joint_name: "wheel_right_joint",
|
||
motor: { vendor: "moons", model: "m2dc10a", gear_ratio: 20, invert: false },
|
||
},
|
||
};
|
||
|
||
let motorCatalog = null;
|
||
const motorWheelsEl = el("motorWheels");
|
||
|
||
async function loadMotorCatalog() {
|
||
if (motorCatalog) return motorCatalog;
|
||
try {
|
||
const res = await fetch("/data/motor_catalog.json", { cache: "no-store" });
|
||
if (!res.ok) throw new Error(`HTTP ${res.status}`);
|
||
motorCatalog = await res.json();
|
||
} catch (e) {
|
||
motorCatalog = {
|
||
vendors: {
|
||
custom: {
|
||
label: "Tùy chỉnh",
|
||
models: {
|
||
custom: {
|
||
label: "Motor tùy chỉnh",
|
||
interface: "other",
|
||
max_rpm: 3000,
|
||
gear_ratio_default: 1,
|
||
},
|
||
},
|
||
},
|
||
},
|
||
};
|
||
setStatus(`Không tải được catalog động cơ: ${e.message}`);
|
||
}
|
||
return motorCatalog;
|
||
}
|
||
|
||
function getVendorOptions() {
|
||
if (!motorCatalog?.vendors) return [];
|
||
return Object.entries(motorCatalog.vendors).map(([id, v]) => ({
|
||
id,
|
||
label: v.label || id,
|
||
}));
|
||
}
|
||
|
||
function getModelOptions(vendorId) {
|
||
const v = motorCatalog?.vendors?.[vendorId];
|
||
if (!v?.models) return [];
|
||
return Object.entries(v.models).map(([id, m]) => ({
|
||
id,
|
||
label: m.label || id,
|
||
}));
|
||
}
|
||
|
||
function getMotorModelSpec(vendorId, modelId) {
|
||
return motorCatalog?.vendors?.[vendorId]?.models?.[modelId] || null;
|
||
}
|
||
|
||
function getMotorDisplayLabel(vendorId, modelId) {
|
||
const v = motorCatalog?.vendors?.[vendorId];
|
||
const m = v?.models?.[modelId];
|
||
if (!v && !m) return "—";
|
||
if (v && m) return `${v.label} — ${m.label}`;
|
||
return v?.label || modelId || "—";
|
||
}
|
||
|
||
function syncWheelPositionsFromSeparation() {
|
||
const diff = state.layout.robot.diff;
|
||
if (!Array.isArray(diff.wheels)) return;
|
||
const half = Number(diff.wheel_separation_m) / 2;
|
||
diff.wheels.forEach((w) => {
|
||
if (w.side === "left" || w.id === "left") w.y_m = half;
|
||
if (w.side === "right" || w.id === "right") w.y_m = -half;
|
||
});
|
||
}
|
||
|
||
function ensureDiffWheels() {
|
||
const diff = state.layout.robot.diff;
|
||
if (!Array.isArray(diff.wheels) || diff.wheels.length < 2) {
|
||
const half = Number(diff.wheel_separation_m || 1) / 2;
|
||
diff.wheels = [
|
||
{ ...DEFAULT_WHEEL_MOTORS.left, motor: { ...DEFAULT_WHEEL_MOTORS.left.motor }, y_m: half },
|
||
{ ...DEFAULT_WHEEL_MOTORS.right, motor: { ...DEFAULT_WHEEL_MOTORS.right.motor }, y_m: -half },
|
||
];
|
||
}
|
||
diff.wheels.forEach((w, idx) => {
|
||
if (!w.id) w.id = idx === 0 ? "left" : "right";
|
||
if (!w.side) w.side = w.id === "right" ? "right" : "left";
|
||
if (!w.motor || typeof w.motor !== "object") {
|
||
const def = w.side === "right" ? DEFAULT_WHEEL_MOTORS.right : DEFAULT_WHEEL_MOTORS.left;
|
||
w.motor = { ...def.motor };
|
||
}
|
||
if (!w.motor.vendor) w.motor.vendor = "custom";
|
||
if (!w.motor.model) w.motor.model = "custom";
|
||
if (w.motor.gear_ratio === undefined) {
|
||
const spec = getMotorModelSpec(w.motor.vendor, w.motor.model);
|
||
w.motor.gear_ratio = spec?.gear_ratio_default ?? 1;
|
||
}
|
||
if (w.motor.invert === undefined) w.motor.invert = false;
|
||
if (!w.joint_name) {
|
||
w.joint_name = w.side === "right" ? "wheel_right_joint" : "wheel_left_joint";
|
||
}
|
||
if (w.y_m === undefined || w.y_m === null) {
|
||
w.y_m = w.side === "right" ? -halfFromSep(diff) : halfFromSep(diff);
|
||
}
|
||
});
|
||
syncWheelPositionsFromSeparation();
|
||
}
|
||
|
||
function halfFromSep(diff) {
|
||
return Number(diff?.wheel_separation_m ?? 1) / 2;
|
||
}
|
||
|
||
function getDiffWheelsForDraw() {
|
||
ensureDiffSchema();
|
||
return state.layout.robot.diff.wheels || [];
|
||
}
|
||
|
||
function renderMotorWheels() {
|
||
if (!motorWheelsEl) return;
|
||
ensureDiffSchema();
|
||
const wheels = state.layout.robot.diff.wheels;
|
||
const vendors = getVendorOptions();
|
||
|
||
motorWheelsEl.innerHTML = wheels
|
||
.map((w) => {
|
||
const sideLabel = w.side === "right" ? "Bánh phải" : "Bánh trái";
|
||
const vendor = w.motor?.vendor || "custom";
|
||
const model = w.motor?.model || "custom";
|
||
const vendorOpts = vendors
|
||
.map(
|
||
(v) =>
|
||
`<option value="${escapeHtml(v.id)}"${v.id === vendor ? " selected" : ""}>${escapeHtml(v.label)}</option>`,
|
||
)
|
||
.join("");
|
||
const models = getModelOptions(vendor);
|
||
const modelOpts = models
|
||
.map(
|
||
(m) =>
|
||
`<option value="${escapeHtml(m.id)}"${m.id === model ? " selected" : ""}>${escapeHtml(m.label)}</option>`,
|
||
)
|
||
.join("");
|
||
const spec = getMotorModelSpec(vendor, model);
|
||
const specTxt = spec
|
||
? `${spec.interface || "—"} • ${spec.max_rpm ?? "—"} rpm • τ≈${spec.rated_torque_nm ?? "—"} Nm`
|
||
: "—";
|
||
return `
|
||
<div class="wheelMotorBlock" data-wheel-id="${escapeHtml(w.id)}">
|
||
<div class="wheelMotorTitle">${escapeHtml(sideLabel)} <span class="mutedNote">(${escapeHtml(w.joint_name || "")})</span></div>
|
||
<div class="row rowWide">
|
||
<label>Hãng</label>
|
||
<select class="motorVendor" data-wheel-id="${escapeHtml(w.id)}">${vendorOpts}</select>
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Model</label>
|
||
<select class="motorModel" data-wheel-id="${escapeHtml(w.id)}">${modelOpts}</select>
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Joint (ROS)</label>
|
||
<input class="motorJoint" data-wheel-id="${escapeHtml(w.id)}" type="text" value="${escapeHtml(w.joint_name || "")}" />
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Tỷ số hộp số</label>
|
||
<input class="motorGear" data-wheel-id="${escapeHtml(w.id)}" type="number" min="0.1" max="200" step="0.1" value="${Number(w.motor.gear_ratio ?? 1)}" />
|
||
</div>
|
||
<div class="checkRow">
|
||
<label>
|
||
<input class="motorInvert" data-wheel-id="${escapeHtml(w.id)}" type="checkbox"${w.motor.invert ? " checked" : ""} />
|
||
Đảo chiều quay
|
||
</label>
|
||
</div>
|
||
<div class="wheelMotorMeta">${escapeHtml(specTxt)}</div>
|
||
</div>`;
|
||
})
|
||
.join("");
|
||
}
|
||
|
||
function findWheelById(wheelId) {
|
||
return state.layout.robot.diff.wheels?.find((w) => w.id === wheelId) || null;
|
||
}
|
||
|
||
function applyMotorWheelsFromDOM() {
|
||
if (!motorWheelsEl) return;
|
||
ensureDiffSchema();
|
||
motorWheelsEl.querySelectorAll(".wheelMotorBlock").forEach((block) => {
|
||
const wheelId = block.dataset.wheelId;
|
||
const w = findWheelById(wheelId);
|
||
if (!w) return;
|
||
const vendor = block.querySelector(".motorVendor")?.value || "custom";
|
||
const model = block.querySelector(".motorModel")?.value || "custom";
|
||
const joint = block.querySelector(".motorJoint")?.value?.trim() || "";
|
||
const gear = Number(block.querySelector(".motorGear")?.value);
|
||
const invert = !!block.querySelector(".motorInvert")?.checked;
|
||
w.motor.vendor = vendor;
|
||
w.motor.model = model;
|
||
w.joint_name = joint || (w.side === "right" ? "wheel_right_joint" : "wheel_left_joint");
|
||
w.motor.gear_ratio = clamp(Number.isFinite(gear) ? gear : 1, 0.1, 200);
|
||
w.motor.invert = invert;
|
||
const meta = block.querySelector(".wheelMotorMeta");
|
||
if (meta) {
|
||
const spec = getMotorModelSpec(vendor, model);
|
||
meta.textContent = spec
|
||
? `${spec.interface || "—"} • ${spec.max_rpm ?? "—"} rpm • τ≈${spec.rated_torque_nm ?? "—"} Nm`
|
||
: "—";
|
||
}
|
||
});
|
||
}
|
||
|
||
function initMotorWheelsEvents() {
|
||
if (!motorWheelsEl || motorWheelsEl.dataset.bound === "1") return;
|
||
motorWheelsEl.dataset.bound = "1";
|
||
|
||
motorWheelsEl.addEventListener("change", (evt) => {
|
||
const vendorSel = evt.target.closest(".motorVendor");
|
||
if (vendorSel) {
|
||
const w = findWheelById(vendorSel.dataset.wheelId);
|
||
if (w) {
|
||
const models = getModelOptions(vendorSel.value);
|
||
const first = models[0]?.id || "custom";
|
||
w.motor.vendor = vendorSel.value;
|
||
w.motor.model = first;
|
||
const spec = getMotorModelSpec(w.motor.vendor, w.motor.model);
|
||
if (spec?.gear_ratio_default !== undefined) {
|
||
w.motor.gear_ratio = spec.gear_ratio_default;
|
||
}
|
||
renderMotorWheels();
|
||
}
|
||
applyMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
applyMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
renderCanvas();
|
||
});
|
||
|
||
motorWheelsEl.addEventListener("input", (evt) => {
|
||
if (evt.target.closest(".motorJoint") || evt.target.closest(".motorGear")) {
|
||
applyMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
}
|
||
});
|
||
}
|
||
|
||
function setStatus(msg) {
|
||
statusEl.textContent = msg;
|
||
}
|
||
|
||
async function api(path, opts = {}) {
|
||
const res = await fetch(path, {
|
||
headers: { "Content-Type": "application/json" },
|
||
...opts,
|
||
});
|
||
if (res.status === 204) return null;
|
||
const text = await res.text();
|
||
let data = null;
|
||
try {
|
||
data = text ? JSON.parse(text) : null;
|
||
} catch {
|
||
data = text;
|
||
}
|
||
if (!res.ok) {
|
||
const err = data && data.error ? data.error : `HTTP ${res.status}`;
|
||
throw new Error(err);
|
||
}
|
||
return data;
|
||
}
|
||
|
||
function ensureDefaultPose(id, idx) {
|
||
if (state.layout.lidarPoses[id]) return;
|
||
const angle = (idx / Math.max(1, state.lidars.length)) * Math.PI * 2;
|
||
const radius = 120;
|
||
state.layout.lidarPoses[id] = {
|
||
x: Math.cos(angle) * radius,
|
||
y: -Math.sin(angle) * radius,
|
||
theta_deg: 0,
|
||
};
|
||
}
|
||
|
||
function ensureDefaultImuPose(id, idx) {
|
||
if (!state.layout.imuPoses) state.layout.imuPoses = {};
|
||
if (state.layout.imuPoses[id]) return;
|
||
const n = Math.max(1, state.imus.length);
|
||
const angle = ((idx + 0.5) / n) * Math.PI * 2;
|
||
const radius = 80;
|
||
state.layout.imuPoses[id] = {
|
||
x: Math.cos(angle) * radius * 0.5,
|
||
y: -Math.sin(angle) * radius * 0.5,
|
||
z: 0.1,
|
||
yaw_deg: 0,
|
||
};
|
||
}
|
||
|
||
function getImuPoseAbs(id) {
|
||
const pose = state.layout.imuPoses?.[id] || null;
|
||
if (!pose) return null;
|
||
const abs = robotToAbs(Number(pose.x || 0), Number(pose.y || 0));
|
||
return { ...pose, absX: abs.x, absY: abs.y };
|
||
}
|
||
|
||
function reconcileImuPoses() {
|
||
if (!state.layout.imuPoses) state.layout.imuPoses = {};
|
||
const valid = new Set(state.imus.map((im) => im.id));
|
||
Object.keys(state.layout.imuPoses).forEach((id) => {
|
||
if (!valid.has(id)) delete state.layout.imuPoses[id];
|
||
});
|
||
}
|
||
|
||
function findDuplicateImuFrame(frameId, excludeId = null) {
|
||
const f = String(frameId || "").trim();
|
||
if (!f) return null;
|
||
return (
|
||
state.imus.find(
|
||
(im) => im.id !== excludeId && String(im.frame_id || "").trim() === f,
|
||
) || null
|
||
);
|
||
}
|
||
|
||
function clearCanvasSelection() {
|
||
state.selectedId = null;
|
||
state.selectedImuId = null;
|
||
selectedText.textContent = "none";
|
||
setSelectedRelText();
|
||
}
|
||
|
||
function selectLidarOnCanvas(id) {
|
||
state.selectedImuId = null;
|
||
state.selectedId = id;
|
||
const l = state.lidars.find((x) => x.id === id);
|
||
selectedText.textContent = l ? `LiDAR: ${l.name}` : id;
|
||
setSelectedRelText();
|
||
refreshLidarSelectionUI();
|
||
refreshImuSelectionUI();
|
||
}
|
||
|
||
function selectImuOnCanvas(id) {
|
||
state.selectedId = null;
|
||
state.selectedImuId = id;
|
||
const im = state.imus.find((x) => x.id === id);
|
||
selectedText.textContent = im ? `IMU: ${im.name}` : id;
|
||
setSelectedRelText();
|
||
refreshLidarSelectionUI();
|
||
refreshImuSelectionUI();
|
||
}
|
||
|
||
function yawCanvasRad() {
|
||
// ROS yaw is CCW around +Z (up). Canvas has +Y down, so we flip sign.
|
||
return (-(state.layout.robot.yaw_deg || 0) * Math.PI) / 180;
|
||
}
|
||
|
||
function ensureDiffSchema() {
|
||
const robot = state.layout.robot;
|
||
if (!robot.diff || typeof robot.diff !== "object") robot.diff = {};
|
||
const diff = robot.diff;
|
||
if (!diff.display || typeof diff.display !== "object") diff.display = {};
|
||
const disp = diff.display;
|
||
const scale =
|
||
Number(disp.scale_m_per_px) > 0 ? Number(disp.scale_m_per_px) : DIFF_DEFAULTS.scale_m_per_px;
|
||
disp.scale_m_per_px = scale;
|
||
|
||
if (diff.wheel_separation_m === undefined) {
|
||
diff.wheel_separation_m =
|
||
diff.b !== undefined ? Number(diff.b) * scale : DIFF_DEFAULTS.wheel_separation_m;
|
||
}
|
||
if (diff.wheel_radius_m === undefined) {
|
||
diff.wheel_radius_m =
|
||
diff.d !== undefined ? (Number(diff.d) / 2) * scale : DIFF_DEFAULTS.wheel_radius_m;
|
||
}
|
||
if (diff.wheel_separation_multiplier === undefined) {
|
||
diff.wheel_separation_multiplier = DIFF_DEFAULTS.wheel_separation_multiplier;
|
||
}
|
||
if (diff.wheel_radius_multiplier === undefined) {
|
||
diff.wheel_radius_multiplier = DIFF_DEFAULTS.wheel_radius_multiplier;
|
||
}
|
||
|
||
if (!diff.limits || typeof diff.limits !== "object") diff.limits = {};
|
||
const lim = diff.limits;
|
||
if (lim.cmd_vel_timeout_s === undefined) lim.cmd_vel_timeout_s = DIFF_DEFAULTS.cmd_vel_timeout_s;
|
||
if (!lim.linear || typeof lim.linear !== "object") lim.linear = {};
|
||
if (lim.linear.max_velocity === undefined) lim.linear.max_velocity = DIFF_DEFAULTS.linear.max_velocity;
|
||
if (lim.linear.min_velocity === undefined) lim.linear.min_velocity = DIFF_DEFAULTS.linear.min_velocity;
|
||
if (lim.linear.max_acceleration === undefined) {
|
||
lim.linear.max_acceleration = DIFF_DEFAULTS.linear.max_acceleration;
|
||
}
|
||
if (lim.linear.min_acceleration === undefined) {
|
||
lim.linear.min_acceleration = DIFF_DEFAULTS.linear.min_acceleration;
|
||
}
|
||
if (!lim.angular || typeof lim.angular !== "object") lim.angular = {};
|
||
if (lim.angular.max_velocity === undefined) {
|
||
lim.angular.max_velocity = DIFF_DEFAULTS.angular.max_velocity;
|
||
}
|
||
if (lim.angular.max_acceleration === undefined) {
|
||
lim.angular.max_acceleration = DIFF_DEFAULTS.angular.max_acceleration;
|
||
}
|
||
|
||
if (!robot.frame_id) robot.frame_id = DIFF_DEFAULTS.frame_id;
|
||
applyDiffDisplayPx();
|
||
ensureDiffWheels();
|
||
}
|
||
|
||
function applyDiffDisplayPx() {
|
||
const diff = state.layout.robot.diff;
|
||
const s = diff.display.scale_m_per_px;
|
||
const bMult = Number(diff.wheel_separation_multiplier) || 1;
|
||
const rMult = Number(diff.wheel_radius_multiplier) || 1;
|
||
diff.display.b_px = (Number(diff.wheel_separation_m) * bMult) / s;
|
||
diff.display.d_px = (2 * Number(diff.wheel_radius_m) * rMult) / s;
|
||
diff.b = diff.display.b_px;
|
||
diff.d = diff.display.d_px;
|
||
}
|
||
|
||
function syncDiffDisplayPx() {
|
||
ensureDiffSchema();
|
||
}
|
||
|
||
function getWheelSeparationPx() {
|
||
syncDiffDisplayPx();
|
||
return state.layout.robot.diff.display.b_px;
|
||
}
|
||
|
||
function getWheelDiameterPx() {
|
||
syncDiffDisplayPx();
|
||
return state.layout.robot.diff.display.d_px;
|
||
}
|
||
|
||
function validateDiff() {
|
||
ensureDiffSchema();
|
||
const diff = state.layout.robot.diff;
|
||
const b = Number(diff.wheel_separation_m);
|
||
const r = Number(diff.wheel_radius_m);
|
||
const msgs = [];
|
||
if (!(b > 2 * r)) msgs.push("Khoảng cách 2 bánh nên lớn hơn đường kính bánh (b > 2r).");
|
||
const lim = diff.limits.linear;
|
||
if (lim.min_velocity > 0) msgs.push("Linear min_velocity thường ≤ 0.");
|
||
if (lim.max_velocity < lim.min_velocity) msgs.push("Linear max_velocity phải ≥ min_velocity.");
|
||
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
let minY = Infinity;
|
||
let maxY = -Infinity;
|
||
fp.forEach((p) => {
|
||
const y = Number(p.y);
|
||
if (Number.isFinite(y)) {
|
||
minY = Math.min(minY, y);
|
||
maxY = Math.max(maxY, y);
|
||
}
|
||
});
|
||
const trackPx = getWheelSeparationPx();
|
||
if (Number.isFinite(minY) && maxY - minY < trackPx * 0.9) {
|
||
msgs.push("Footprint có vẻ hẹp hơn khoảng cách 2 bánh — kiểm tra lại.");
|
||
}
|
||
|
||
if (msgs.length === 0) {
|
||
diffValidationEl.hidden = true;
|
||
diffValidationEl.textContent = "";
|
||
diffValidationEl.classList.remove("error");
|
||
return true;
|
||
}
|
||
diffValidationEl.hidden = false;
|
||
diffValidationEl.textContent = msgs.join(" ");
|
||
diffValidationEl.classList.toggle("error", msgs.some((m) => m.includes("phải")));
|
||
return false;
|
||
}
|
||
|
||
function updateRobotDiffSummary() {
|
||
const model = state.layout.robot.model || "diff";
|
||
if (model === "bicycle") {
|
||
ensureBicycleSchema();
|
||
const b = state.layout.robot.bicycle;
|
||
const lim = b.limits.linear;
|
||
const rear = b.wheels?.find((w) => w.id === "rear" || w.role === "drive");
|
||
const front = b.wheels?.find((w) => w.id === "front" || w.role === "steer");
|
||
const rMot = rear?.motor ? getMotorDisplayLabel(rear.motor.vendor, rear.motor.model) : "—";
|
||
const fMot = front?.motor ? getMotorDisplayLabel(front.motor.vendor, front.motor.model) : "—";
|
||
const delta = Number(b.steer?.preview_deg ?? 0);
|
||
const R = Math.abs(delta) > 0.5 ? Number(b.wheelbase_m) / Math.tan((delta * Math.PI) / 180) : Infinity;
|
||
const rTxt = Number.isFinite(R) && R < 1e4 ? `R≈${R.toFixed(2)}m` : "R=∞";
|
||
robotDiffSummaryEl.textContent =
|
||
`Bicycle: L=${Number(b.wheelbase_m).toFixed(2)} m, δ=${delta.toFixed(0)}° (${rTxt}) | ` +
|
||
`v≤${Number(lim.max_velocity).toFixed(1)} m/s | rear: ${rMot} | steer: ${fMot}`;
|
||
return;
|
||
}
|
||
if (model !== "diff") {
|
||
robotDiffSummaryEl.textContent = `Model: ${model}`;
|
||
return;
|
||
}
|
||
ensureDiffSchema();
|
||
const d = state.layout.robot.diff;
|
||
const lim = d.limits.linear;
|
||
const wheels = d.wheels || [];
|
||
const left = wheels.find((w) => w.side === "left" || w.id === "left");
|
||
const right = wheels.find((w) => w.side === "right" || w.id === "right");
|
||
const lMot = left?.motor ? getMotorDisplayLabel(left.motor.vendor, left.motor.model) : "—";
|
||
const rMot = right?.motor ? getMotorDisplayLabel(right.motor.vendor, right.motor.model) : "—";
|
||
robotDiffSummaryEl.textContent =
|
||
`Diff: b=${Number(d.wheel_separation_m).toFixed(2)} m, r=${Number(d.wheel_radius_m).toFixed(2)} m | ` +
|
||
`v≤${Number(lim.max_velocity).toFixed(1)} m/s | L: ${lMot} | R: ${rMot}`;
|
||
}
|
||
|
||
function ensureBicycleSchema() {
|
||
const robot = state.layout.robot;
|
||
if (!robot.bicycle || typeof robot.bicycle !== "object") robot.bicycle = {};
|
||
const b = robot.bicycle;
|
||
if (!b.display || typeof b.display !== "object") b.display = {};
|
||
const scale =
|
||
Number(b.display.scale_m_per_px) > 0 ? Number(b.display.scale_m_per_px) : BICYCLE_DEFAULTS.scale_m_per_px;
|
||
b.display.scale_m_per_px = scale;
|
||
|
||
if (b.wheelbase_m === undefined) b.wheelbase_m = BICYCLE_DEFAULTS.wheelbase_m;
|
||
if (b.wheel_radius_m === undefined) b.wheel_radius_m = BICYCLE_DEFAULTS.wheel_radius_m;
|
||
|
||
if (!b.steer || typeof b.steer !== "object") b.steer = {};
|
||
if (b.steer.max_angle_deg === undefined) b.steer.max_angle_deg = BICYCLE_DEFAULTS.steer_max_deg;
|
||
if (b.steer.preview_deg === undefined) b.steer.preview_deg = BICYCLE_DEFAULTS.steer_preview_deg;
|
||
if (!b.steer.joint_name) b.steer.joint_name = "front_steer_joint";
|
||
|
||
if (!b.drive || typeof b.drive !== "object") b.drive = {};
|
||
if (!b.drive.joint_name) b.drive.joint_name = "rear_wheel_joint";
|
||
|
||
if (!b.limits || typeof b.limits !== "object") b.limits = {};
|
||
const lim = b.limits;
|
||
if (lim.cmd_vel_timeout_s === undefined) lim.cmd_vel_timeout_s = BICYCLE_DEFAULTS.cmd_vel_timeout_s;
|
||
if (!lim.linear || typeof lim.linear !== "object") lim.linear = {};
|
||
if (lim.linear.max_velocity === undefined) lim.linear.max_velocity = BICYCLE_DEFAULTS.linear_max_velocity;
|
||
if (lim.linear.max_acceleration === undefined) {
|
||
lim.linear.max_acceleration = BICYCLE_DEFAULTS.linear_max_acceleration;
|
||
}
|
||
|
||
ensureBicycleWheels();
|
||
applyBicycleDisplayPx();
|
||
if (!robot.frame_id) robot.frame_id = BICYCLE_DEFAULTS.frame_id;
|
||
}
|
||
|
||
function ensureBicycleWheels() {
|
||
const b = state.layout.robot.bicycle;
|
||
const L = Number(b.wheelbase_m) || BICYCLE_DEFAULTS.wheelbase_m;
|
||
if (!Array.isArray(b.wheels) || b.wheels.length < 2) {
|
||
b.wheels = [
|
||
{ ...DEFAULT_BICYCLE_WHEELS.rear, motor: { ...DEFAULT_BICYCLE_WHEELS.rear.motor } },
|
||
{
|
||
...DEFAULT_BICYCLE_WHEELS.front,
|
||
x_m: L,
|
||
motor: { ...DEFAULT_BICYCLE_WHEELS.front.motor },
|
||
},
|
||
];
|
||
}
|
||
b.wheels.forEach((w) => {
|
||
if (!w.id) w.id = w.role === "steer" ? "front" : "rear";
|
||
if (!w.role) w.role = w.id === "front" ? "steer" : "drive";
|
||
if (!w.motor || typeof w.motor !== "object") {
|
||
const def = w.role === "steer" ? DEFAULT_BICYCLE_WHEELS.front : DEFAULT_BICYCLE_WHEELS.rear;
|
||
w.motor = { ...def.motor };
|
||
}
|
||
if (!w.motor.vendor) w.motor.vendor = "custom";
|
||
if (!w.motor.model) w.motor.model = "custom";
|
||
if (w.motor.gear_ratio === undefined) {
|
||
const spec = getMotorModelSpec(w.motor.vendor, w.motor.model);
|
||
w.motor.gear_ratio = spec?.gear_ratio_default ?? 1;
|
||
}
|
||
if (w.motor.invert === undefined) w.motor.invert = false;
|
||
if (!w.joint_name) {
|
||
w.joint_name = w.role === "steer" ? b.steer.joint_name : b.drive.joint_name;
|
||
}
|
||
if (w.role === "steer" || w.id === "front") {
|
||
w.x_m = L;
|
||
w.y_m = 0;
|
||
} else {
|
||
w.x_m = 0;
|
||
w.y_m = 0;
|
||
}
|
||
});
|
||
}
|
||
|
||
function applyBicycleDisplayPx() {
|
||
const b = state.layout.robot.bicycle;
|
||
const s = b.display.scale_m_per_px;
|
||
b.display.L_px = Number(b.wheelbase_m) / s;
|
||
b.display.r_px = (2 * Number(b.wheel_radius_m)) / s;
|
||
}
|
||
|
||
function getBicycleWheelbasePx() {
|
||
ensureBicycleSchema();
|
||
return state.layout.robot.bicycle.display.L_px;
|
||
}
|
||
|
||
function getBicycleWheelDiameterPx() {
|
||
ensureBicycleSchema();
|
||
return state.layout.robot.bicycle.display.r_px;
|
||
}
|
||
|
||
function validateBicycle() {
|
||
ensureBicycleSchema();
|
||
const b = state.layout.robot.bicycle;
|
||
const L = Number(b.wheelbase_m);
|
||
const r = Number(b.wheel_radius_m);
|
||
const msgs = [];
|
||
if (!(L > 2 * r)) msgs.push("Wheelbase L nên lớn hơn đường kính bánh (L > 2r).");
|
||
const maxDeg = Number(b.steer.max_angle_deg);
|
||
const preview = Number(b.steer.preview_deg);
|
||
if (Math.abs(preview) > maxDeg) msgs.push("Góc xem trước vượt δ max.");
|
||
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
let minX = Infinity;
|
||
let maxX = -Infinity;
|
||
fp.forEach((p) => {
|
||
const x = Number(p.x);
|
||
if (Number.isFinite(x)) {
|
||
minX = Math.min(minX, x);
|
||
maxX = Math.max(maxX, x);
|
||
}
|
||
});
|
||
const Lpx = getBicycleWheelbasePx();
|
||
if (Number.isFinite(minX) && maxX - minX < Lpx * 0.85) {
|
||
msgs.push("Footprint có vẻ ngắn hơn wheelbase — kiểm tra lại.");
|
||
}
|
||
|
||
if (msgs.length === 0) {
|
||
bicycleValidationEl.hidden = true;
|
||
bicycleValidationEl.textContent = "";
|
||
return true;
|
||
}
|
||
bicycleValidationEl.hidden = false;
|
||
bicycleValidationEl.textContent = msgs.join(" ");
|
||
bicycleValidationEl.classList.add("error");
|
||
return false;
|
||
}
|
||
|
||
function syncBicycleFormFromState() {
|
||
ensureBicycleSchema();
|
||
const b = state.layout.robot.bicycle;
|
||
const lim = b.limits;
|
||
|
||
bicycleWheelbaseMEl.value = Number(b.wheelbase_m).toFixed(3);
|
||
bicycleWheelRadiusMEl.value = Number(b.wheel_radius_m).toFixed(3);
|
||
bicycleScaleMPerPxEl.value = Number(b.display.scale_m_per_px).toFixed(4);
|
||
bicycleSteerPreviewDegEl.value = Number(b.steer.preview_deg).toFixed(0);
|
||
bicycleSteerMaxDegEl.value = Number(b.steer.max_angle_deg).toFixed(0);
|
||
bicycleCmdVelTimeoutEl.value = Number(lim.cmd_vel_timeout_s).toFixed(2);
|
||
bicycleLinearMaxVelEl.value = Number(lim.linear.max_velocity).toFixed(2);
|
||
bicycleLinearMaxAccelEl.value = Number(lim.linear.max_acceleration).toFixed(2);
|
||
|
||
renderBicycleMotorWheels();
|
||
validateBicycle();
|
||
}
|
||
|
||
function applyBicycleFormToState() {
|
||
ensureBicycleSchema();
|
||
const robot = state.layout.robot;
|
||
const b = robot.bicycle;
|
||
const lim = b.limits;
|
||
|
||
robot.model = "bicycle";
|
||
if (!robot.frame_id) robot.frame_id = BICYCLE_DEFAULTS.frame_id;
|
||
b.wheelbase_m = clamp(Number(bicycleWheelbaseMEl.value), 0.2, 5);
|
||
b.wheel_radius_m = clamp(Number(bicycleWheelRadiusMEl.value), 0.02, 1);
|
||
b.display.scale_m_per_px = clamp(Number(bicycleScaleMPerPxEl.value), 0.001, 0.1);
|
||
b.steer.preview_deg = clamp(Number(bicycleSteerPreviewDegEl.value), -60, 60);
|
||
b.steer.max_angle_deg = clamp(Number(bicycleSteerMaxDegEl.value), 5, 60);
|
||
lim.cmd_vel_timeout_s = clamp(Number(bicycleCmdVelTimeoutEl.value), 0.05, 5);
|
||
lim.linear.max_velocity = clamp(Number(bicycleLinearMaxVelEl.value), 0.01, 5);
|
||
lim.linear.max_acceleration = clamp(Number(bicycleLinearMaxAccelEl.value), 0.01, 10);
|
||
|
||
ensureBicycleWheels();
|
||
applyBicycleMotorWheelsFromDOM();
|
||
applyBicycleDisplayPx();
|
||
validateBicycle();
|
||
updateRobotDiffSummary();
|
||
}
|
||
|
||
function onBicycleFieldChange() {
|
||
applyBicycleFormToState();
|
||
markLayoutDirty();
|
||
renderCanvas();
|
||
}
|
||
|
||
function findBicycleWheelById(wheelId) {
|
||
return state.layout.robot.bicycle?.wheels?.find((w) => w.id === wheelId) || null;
|
||
}
|
||
|
||
function renderBicycleMotorWheels() {
|
||
if (!bicycleMotorWheelsEl) return;
|
||
ensureBicycleSchema();
|
||
const wheels = state.layout.robot.bicycle.wheels;
|
||
const vendors = getVendorOptions();
|
||
|
||
bicycleMotorWheelsEl.innerHTML = wheels
|
||
.map((w) => {
|
||
const roleLabel = w.role === "steer" ? "Bánh trước (steer)" : "Bánh sau (drive)";
|
||
const vendor = w.motor?.vendor || "custom";
|
||
const model = w.motor?.model || "custom";
|
||
const vendorOpts = vendors
|
||
.map(
|
||
(v) =>
|
||
`<option value="${escapeHtml(v.id)}"${v.id === vendor ? " selected" : ""}>${escapeHtml(v.label)}</option>`,
|
||
)
|
||
.join("");
|
||
const models = getModelOptions(vendor);
|
||
const modelOpts = models
|
||
.map(
|
||
(m) =>
|
||
`<option value="${escapeHtml(m.id)}"${m.id === model ? " selected" : ""}>${escapeHtml(m.label)}</option>`,
|
||
)
|
||
.join("");
|
||
const spec = getMotorModelSpec(vendor, model);
|
||
const specTxt = spec
|
||
? `${spec.interface || "—"} • ${spec.max_rpm ?? "—"} rpm`
|
||
: "—";
|
||
return `
|
||
<div class="wheelMotorBlock" data-wheel-id="${escapeHtml(w.id)}">
|
||
<div class="wheelMotorTitle">${escapeHtml(roleLabel)} <span class="mutedNote">(${escapeHtml(w.joint_name || "")})</span></div>
|
||
<div class="row rowWide">
|
||
<label>Hãng</label>
|
||
<select class="motorVendor" data-wheel-id="${escapeHtml(w.id)}">${vendorOpts}</select>
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Model</label>
|
||
<select class="motorModel" data-wheel-id="${escapeHtml(w.id)}">${modelOpts}</select>
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Joint (ROS)</label>
|
||
<input class="motorJoint" data-wheel-id="${escapeHtml(w.id)}" type="text" value="${escapeHtml(w.joint_name || "")}" />
|
||
</div>
|
||
<div class="row rowWide">
|
||
<label>Tỷ số hộp số</label>
|
||
<input class="motorGear" data-wheel-id="${escapeHtml(w.id)}" type="number" min="0.1" max="200" step="0.1" value="${Number(w.motor.gear_ratio ?? 1)}" />
|
||
</div>
|
||
<div class="checkRow">
|
||
<label>
|
||
<input class="motorInvert" data-wheel-id="${escapeHtml(w.id)}" type="checkbox"${w.motor.invert ? " checked" : ""} />
|
||
Đảo chiều
|
||
</label>
|
||
</div>
|
||
<div class="wheelMotorMeta">${escapeHtml(specTxt)}</div>
|
||
</div>`;
|
||
})
|
||
.join("");
|
||
}
|
||
|
||
function applyBicycleMotorWheelsFromDOM() {
|
||
if (!bicycleMotorWheelsEl) return;
|
||
ensureBicycleSchema();
|
||
const b = state.layout.robot.bicycle;
|
||
bicycleMotorWheelsEl.querySelectorAll(".wheelMotorBlock").forEach((block) => {
|
||
const wheelId = block.dataset.wheelId;
|
||
const w = findBicycleWheelById(wheelId);
|
||
if (!w) return;
|
||
const vendor = block.querySelector(".motorVendor")?.value || "custom";
|
||
const model = block.querySelector(".motorModel")?.value || "custom";
|
||
const joint = block.querySelector(".motorJoint")?.value?.trim() || "";
|
||
const gear = Number(block.querySelector(".motorGear")?.value);
|
||
const invert = !!block.querySelector(".motorInvert")?.checked;
|
||
w.motor.vendor = vendor;
|
||
w.motor.model = model;
|
||
w.joint_name = joint || (w.role === "steer" ? b.steer.joint_name : b.drive.joint_name);
|
||
w.motor.gear_ratio = clamp(Number.isFinite(gear) ? gear : 1, 0.1, 200);
|
||
w.motor.invert = invert;
|
||
if (w.role === "steer") b.steer.joint_name = w.joint_name;
|
||
else b.drive.joint_name = w.joint_name;
|
||
});
|
||
}
|
||
|
||
function initBicycleMotorWheelsEvents() {
|
||
if (!bicycleMotorWheelsEl || bicycleMotorWheelsEl.dataset.bound === "1") return;
|
||
bicycleMotorWheelsEl.dataset.bound = "1";
|
||
|
||
bicycleMotorWheelsEl.addEventListener("change", (evt) => {
|
||
const vendorSel = evt.target.closest(".motorVendor");
|
||
if (vendorSel) {
|
||
const w = findBicycleWheelById(vendorSel.dataset.wheelId);
|
||
if (w) {
|
||
const models = getModelOptions(vendorSel.value);
|
||
w.motor.vendor = vendorSel.value;
|
||
w.motor.model = models[0]?.id || "custom";
|
||
const spec = getMotorModelSpec(w.motor.vendor, w.motor.model);
|
||
if (spec?.gear_ratio_default !== undefined) w.motor.gear_ratio = spec.gear_ratio_default;
|
||
renderBicycleMotorWheels();
|
||
}
|
||
applyBicycleMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
return;
|
||
}
|
||
applyBicycleMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
});
|
||
|
||
bicycleMotorWheelsEl.addEventListener("input", (evt) => {
|
||
if (evt.target.closest(".motorJoint") || evt.target.closest(".motorGear")) {
|
||
applyBicycleMotorWheelsFromDOM();
|
||
updateRobotDiffSummary();
|
||
}
|
||
});
|
||
}
|
||
|
||
function syncDiffFormFromState() {
|
||
ensureDiffSchema();
|
||
const robot = state.layout.robot;
|
||
const d = robot.diff;
|
||
const lim = d.limits;
|
||
|
||
robotModelEl.value = robot.model || "diff";
|
||
wheelSeparationMEl.value = Number(d.wheel_separation_m).toFixed(3);
|
||
wheelRadiusMEl.value = Number(d.wheel_radius_m).toFixed(3);
|
||
scaleMPerPxEl.value = Number(d.display.scale_m_per_px).toFixed(4);
|
||
wheelSeparationMultEl.value = Number(d.wheel_separation_multiplier).toFixed(2);
|
||
wheelRadiusMultEl.value = Number(d.wheel_radius_multiplier).toFixed(2);
|
||
cmdVelTimeoutEl.value = Number(lim.cmd_vel_timeout_s).toFixed(2);
|
||
linearMaxVelEl.value = Number(lim.linear.max_velocity).toFixed(2);
|
||
linearMinVelEl.value = Number(lim.linear.min_velocity).toFixed(2);
|
||
linearMaxAccelEl.value = Number(lim.linear.max_acceleration).toFixed(2);
|
||
angularMaxVelEl.value = Number(lim.angular.max_velocity).toFixed(2);
|
||
angularMaxAccelEl.value = Number(lim.angular.max_acceleration).toFixed(2);
|
||
|
||
const isDiff = robotModelEl.value === "diff";
|
||
const isBicycle = robotModelEl.value === "bicycle";
|
||
diffParamsEl.hidden = !isDiff;
|
||
bicycleParamsEl.hidden = !isBicycle;
|
||
if (diffValidationEl) diffValidationEl.hidden = !isDiff;
|
||
if (bicycleValidationEl) bicycleValidationEl.hidden = !isBicycle;
|
||
|
||
if (isDiff) {
|
||
renderMotorWheels();
|
||
validateDiff();
|
||
} else if (isBicycle) {
|
||
syncBicycleFormFromState();
|
||
}
|
||
updateRobotDiffSummary();
|
||
}
|
||
|
||
function applyDiffFormToState() {
|
||
ensureDiffSchema();
|
||
const robot = state.layout.robot;
|
||
const d = robot.diff;
|
||
const lim = d.limits;
|
||
|
||
robot.model = robotModelEl.value || "diff";
|
||
if (!robot.frame_id) robot.frame_id = DIFF_DEFAULTS.frame_id;
|
||
d.wheel_separation_m = clamp(Number(wheelSeparationMEl.value), 0.05, 5);
|
||
d.wheel_radius_m = clamp(Number(wheelRadiusMEl.value), 0.02, 1);
|
||
d.display.scale_m_per_px = clamp(Number(scaleMPerPxEl.value), 0.001, 0.1);
|
||
d.wheel_separation_multiplier = clamp(Number(wheelSeparationMultEl.value), 0.5, 2);
|
||
d.wheel_radius_multiplier = clamp(Number(wheelRadiusMultEl.value), 0.5, 2);
|
||
lim.cmd_vel_timeout_s = clamp(Number(cmdVelTimeoutEl.value), 0.05, 5);
|
||
lim.linear.max_velocity = clamp(Number(linearMaxVelEl.value), 0.01, 5);
|
||
lim.linear.min_velocity = clamp(Number(linearMinVelEl.value), -5, 0);
|
||
lim.linear.max_acceleration = clamp(Number(linearMaxAccelEl.value), 0.01, 10);
|
||
lim.linear.min_acceleration = -Math.abs(lim.linear.max_acceleration);
|
||
lim.angular.max_velocity = clamp(Number(angularMaxVelEl.value), 0.01, 10);
|
||
lim.angular.max_acceleration = clamp(Number(angularMaxAccelEl.value), 0.01, 10);
|
||
|
||
syncWheelPositionsFromSeparation();
|
||
applyMotorWheelsFromDOM();
|
||
syncDiffDisplayPx();
|
||
validateDiff();
|
||
updateRobotDiffSummary();
|
||
}
|
||
|
||
function onDiffFieldChange() {
|
||
applyDiffFormToState();
|
||
markLayoutDirty();
|
||
renderCanvas();
|
||
}
|
||
|
||
function robotToAbs(xRobot, yRobot) {
|
||
// Robot frame (ROS REP-103): x forward, y left.
|
||
// Convert robot-frame (x,y) into canvas absolute coordinates.
|
||
const r = state.layout.robot;
|
||
const yaw = yawCanvasRad();
|
||
const xh = { x: Math.cos(yaw), y: Math.sin(yaw) };
|
||
const yh = { x: Math.cos(yaw - Math.PI / 2), y: Math.sin(yaw - Math.PI / 2) };
|
||
return {
|
||
x: r.x + xRobot * xh.x + yRobot * yh.x,
|
||
y: r.y + xRobot * xh.y + yRobot * yh.y,
|
||
};
|
||
}
|
||
|
||
function absToRobot(xAbs, yAbs) {
|
||
const r = state.layout.robot;
|
||
const dx = xAbs - r.x;
|
||
const dy = yAbs - r.y;
|
||
const yaw = yawCanvasRad();
|
||
const xh = { x: Math.cos(yaw), y: Math.sin(yaw) };
|
||
const yh = { x: Math.cos(yaw - Math.PI / 2), y: Math.sin(yaw - Math.PI / 2) };
|
||
return {
|
||
x: dx * xh.x + dy * xh.y,
|
||
y: dx * yh.x + dy * yh.y,
|
||
};
|
||
}
|
||
|
||
function getLidarPoseAbs(id) {
|
||
const pose = state.layout.lidarPoses[id] || null;
|
||
if (!pose) return null;
|
||
const abs = robotToAbs(Number(pose.x || 0), Number(pose.y || 0));
|
||
return { ...pose, absX: abs.x, absY: abs.y };
|
||
}
|
||
|
||
const FOOTPRINT_DEFAULT_PARAMS = {
|
||
length_m: 1.4,
|
||
width_m: 1.1,
|
||
radius_m: 0.55,
|
||
sides: 6,
|
||
segments: 32,
|
||
};
|
||
|
||
function getScaleMPerPx() {
|
||
const robot = state.layout.robot;
|
||
if ((robot.model || "diff") === "bicycle") {
|
||
return Number(robot.bicycle?.display?.scale_m_per_px) || BICYCLE_DEFAULTS.scale_m_per_px;
|
||
}
|
||
return Number(robot.diff?.display?.scale_m_per_px) || DIFF_DEFAULTS.scale_m_per_px;
|
||
}
|
||
|
||
function mToRobotPx(m) {
|
||
return Number(m) / getScaleMPerPx();
|
||
}
|
||
|
||
function robotPxToM(px) {
|
||
return Number(px) * getScaleMPerPx();
|
||
}
|
||
|
||
function isCustomFootprintShape() {
|
||
return (state.layout.robot.footprint_shape || "custom") === "custom";
|
||
}
|
||
|
||
function ensureFootprintSchema() {
|
||
const robot = state.layout.robot;
|
||
if (!robot.footprint_shape) robot.footprint_shape = "custom";
|
||
if (!robot.footprint_params || typeof robot.footprint_params !== "object") {
|
||
robot.footprint_params = { ...FOOTPRINT_DEFAULT_PARAMS };
|
||
}
|
||
const p = robot.footprint_params;
|
||
if (p.length_m === undefined) p.length_m = FOOTPRINT_DEFAULT_PARAMS.length_m;
|
||
if (p.width_m === undefined) p.width_m = FOOTPRINT_DEFAULT_PARAMS.width_m;
|
||
if (p.radius_m === undefined) p.radius_m = FOOTPRINT_DEFAULT_PARAMS.radius_m;
|
||
if (p.sides === undefined) p.sides = FOOTPRINT_DEFAULT_PARAMS.sides;
|
||
if (p.segments === undefined) p.segments = FOOTPRINT_DEFAULT_PARAMS.segments;
|
||
}
|
||
|
||
function footprintBoundsPx(points) {
|
||
let minX = Infinity;
|
||
let maxX = -Infinity;
|
||
let minY = Infinity;
|
||
let maxY = -Infinity;
|
||
points.forEach((pt) => {
|
||
const x = Number(pt.x);
|
||
const y = Number(pt.y);
|
||
if (!Number.isFinite(x) || !Number.isFinite(y)) return;
|
||
minX = Math.min(minX, x);
|
||
maxX = Math.max(maxX, x);
|
||
minY = Math.min(minY, y);
|
||
maxY = Math.max(maxY, y);
|
||
});
|
||
if (!Number.isFinite(minX)) return null;
|
||
return { minX, maxX, minY, maxY };
|
||
}
|
||
|
||
function inferFootprintParamsFromPoints(points) {
|
||
const b = footprintBoundsPx(points);
|
||
if (!b) return { ...FOOTPRINT_DEFAULT_PARAMS };
|
||
const lengthPx = b.maxX - b.minX;
|
||
const widthPx = b.maxY - b.minY;
|
||
const radiusPx = Math.max(lengthPx, widthPx) / 2;
|
||
return {
|
||
length_m: robotPxToM(lengthPx),
|
||
width_m: robotPxToM(widthPx),
|
||
radius_m: robotPxToM(radiusPx),
|
||
sides: FOOTPRINT_DEFAULT_PARAMS.sides,
|
||
segments: FOOTPRINT_DEFAULT_PARAMS.segments,
|
||
};
|
||
}
|
||
|
||
function regularPolygonPoints(radiusPx, sides) {
|
||
const pts = [];
|
||
const n = clamp(Math.round(sides), 3, 64);
|
||
for (let i = 0; i < n; i++) {
|
||
const a = (2 * Math.PI * i) / n;
|
||
pts.push({ x: radiusPx * Math.cos(a), y: radiusPx * Math.sin(a) });
|
||
}
|
||
return pts;
|
||
}
|
||
|
||
function generateFootprintPoints(shape, params) {
|
||
const p = params || {};
|
||
switch (shape) {
|
||
case "rectangle": {
|
||
const L = mToRobotPx(clamp(Number(p.length_m), 0.1, 20));
|
||
const W = mToRobotPx(clamp(Number(p.width_m), 0.1, 20));
|
||
const hx = L / 2;
|
||
const hy = W / 2;
|
||
return [
|
||
{ x: hx, y: hy },
|
||
{ x: hx, y: -hy },
|
||
{ x: -hx, y: -hy },
|
||
{ x: -hx, y: hy },
|
||
];
|
||
}
|
||
case "circle": {
|
||
const r = mToRobotPx(clamp(Number(p.radius_m), 0.05, 10));
|
||
const n = clamp(Math.round(Number(p.segments) || 32), 8, 64);
|
||
return regularPolygonPoints(r, n);
|
||
}
|
||
case "regular_polygon": {
|
||
const r = mToRobotPx(clamp(Number(p.radius_m), 0.05, 10));
|
||
const sides = clamp(Math.round(Number(p.sides) || 6), 3, 32);
|
||
return regularPolygonPoints(r, sides);
|
||
}
|
||
default:
|
||
return null;
|
||
}
|
||
}
|
||
|
||
function readFootprintParamsFromDOM() {
|
||
const robot = state.layout.robot;
|
||
if (!robot.footprint_params) robot.footprint_params = { ...FOOTPRINT_DEFAULT_PARAMS };
|
||
const p = robot.footprint_params;
|
||
const shape = footprintShapeEl?.value || robot.footprint_shape;
|
||
if (shape === "rectangle") {
|
||
p.length_m = clamp(Number(fpLengthMEl?.value), 0.1, 20);
|
||
p.width_m = clamp(Number(fpWidthMEl?.value), 0.1, 20);
|
||
} else if (shape === "circle") {
|
||
p.radius_m = clamp(Number(fpRadiusMEl?.value), 0.05, 10);
|
||
p.segments = clamp(Math.round(Number(fpCircleSegmentsEl?.value) || 32), 8, 64);
|
||
} else if (shape === "regular_polygon") {
|
||
p.radius_m = clamp(Number(fpPolyRadiusMEl?.value), 0.05, 10);
|
||
p.sides = clamp(Math.round(Number(fpPolySidesEl?.value) || 6), 3, 32);
|
||
}
|
||
return p;
|
||
}
|
||
|
||
function applyFootprintPreset() {
|
||
ensureFootprintSchema();
|
||
const robot = state.layout.robot;
|
||
const shape = footprintShapeEl?.value || robot.footprint_shape;
|
||
if (shape === "custom") {
|
||
setStatus("Chế độ tùy chỉnh — chỉnh đỉnh trên canvas");
|
||
return;
|
||
}
|
||
readFootprintParamsFromDOM();
|
||
robot.footprint_shape = shape;
|
||
const pts = generateFootprintPoints(shape, robot.footprint_params);
|
||
if (!pts || pts.length < 3) {
|
||
setStatus("Không tạo được footprint");
|
||
return;
|
||
}
|
||
robot.footprint = pts;
|
||
state.selectedFootprintVertex = null;
|
||
validateDiff();
|
||
updateFootprintEditHint();
|
||
renderCanvas();
|
||
markLayoutDirty();
|
||
setStatus(`Đã áp dụng footprint: ${footprintShapeLabel(shape)}`);
|
||
}
|
||
|
||
function footprintShapeLabel(shape) {
|
||
const labels = {
|
||
rectangle: "Hình chữ nhật",
|
||
circle: "Hình tròn",
|
||
regular_polygon: "Đa giác đều",
|
||
custom: "Tùy chỉnh",
|
||
};
|
||
return labels[shape] || shape;
|
||
}
|
||
|
||
function updateFootprintPresetPanelVisibility() {
|
||
const shape = footprintShapeEl?.value || state.layout.robot.footprint_shape || "custom";
|
||
const isCustom = shape === "custom";
|
||
if (footprintPresetPanelEl) footprintPresetPanelEl.classList.toggle("hidden", isCustom);
|
||
if (footprintCustomPanelEl) footprintCustomPanelEl.classList.toggle("hidden", !isCustom);
|
||
if (fpRectParamsEl) fpRectParamsEl.hidden = shape !== "rectangle";
|
||
if (fpCircleParamsEl) fpCircleParamsEl.hidden = shape !== "circle";
|
||
if (fpPolyParamsEl) fpPolyParamsEl.hidden = shape !== "regular_polygon";
|
||
updateFootprintVertexUI();
|
||
}
|
||
|
||
function syncFootprintUIFromState() {
|
||
ensureFootprintSchema();
|
||
const robot = state.layout.robot;
|
||
const shape = robot.footprint_shape || "custom";
|
||
const p = robot.footprint_params;
|
||
if (footprintShapeEl) footprintShapeEl.value = shape;
|
||
if (fpLengthMEl) fpLengthMEl.value = Number(p.length_m).toFixed(2);
|
||
if (fpWidthMEl) fpWidthMEl.value = Number(p.width_m).toFixed(2);
|
||
if (fpRadiusMEl) fpRadiusMEl.value = Number(p.radius_m).toFixed(2);
|
||
if (fpCircleSegmentsEl) fpCircleSegmentsEl.value = String(Math.round(p.segments));
|
||
if (fpPolyRadiusMEl) fpPolyRadiusMEl.value = Number(p.radius_m).toFixed(2);
|
||
if (fpPolySidesEl) fpPolySidesEl.value = String(Math.round(p.sides));
|
||
updateFootprintPresetPanelVisibility();
|
||
updateFootprintEditHint();
|
||
}
|
||
|
||
function updateFootprintEditHint() {
|
||
if (!footprintEditHint) return;
|
||
if (!state.editFootprint) return;
|
||
if (isCustomFootprintShape()) {
|
||
footprintEditHint.textContent =
|
||
"Click canvas: thêm đỉnh (trên cạnh = chèn giữa) • Kéo đỉnh • Delete / nút Xóa đỉnh • Esc bỏ chọn";
|
||
} else {
|
||
footprintEditHint.textContent =
|
||
"Chỉnh kích thước ở trên rồi nhấn «Áp dụng hình dạng» — hoặc chuyển sang Tùy chỉnh để kéo từng đỉnh";
|
||
}
|
||
}
|
||
|
||
function onFootprintShapeChange() {
|
||
ensureFootprintSchema();
|
||
const prev = state.layout.robot.footprint_shape || "custom";
|
||
const next = footprintShapeEl.value || "custom";
|
||
state.layout.robot.footprint_shape = next;
|
||
if (next !== "custom" && prev === "custom") {
|
||
state.layout.robot.footprint_params = inferFootprintParamsFromPoints(state.layout.robot.footprint);
|
||
}
|
||
syncFootprintUIFromState();
|
||
if (next !== "custom") applyFootprintPreset();
|
||
else {
|
||
validateDiff();
|
||
renderCanvas();
|
||
setStatus("Chế độ tùy chỉnh — bật «Sửa footprint» để chỉnh đỉnh");
|
||
}
|
||
}
|
||
|
||
function initFootprintEvents() {
|
||
if (footprintShapeEl && footprintShapeEl.dataset.bound !== "1") {
|
||
footprintShapeEl.dataset.bound = "1";
|
||
footprintShapeEl.addEventListener("change", onFootprintShapeChange);
|
||
}
|
||
if (applyFootprintPresetBtn && applyFootprintPresetBtn.dataset.bound !== "1") {
|
||
applyFootprintPresetBtn.dataset.bound = "1";
|
||
applyFootprintPresetBtn.addEventListener("click", () => {
|
||
applyFootprintPreset();
|
||
markLayoutDirty();
|
||
persistLayoutDebounced();
|
||
});
|
||
}
|
||
if (fpAddVertexBtn && fpAddVertexBtn.dataset.bound !== "1") {
|
||
fpAddVertexBtn.dataset.bound = "1";
|
||
fpAddVertexBtn.addEventListener("click", () => {
|
||
addFootprintVertexFromUI();
|
||
persistLayoutDebounced();
|
||
});
|
||
}
|
||
if (fpRemoveVertexBtn && fpRemoveVertexBtn.dataset.bound !== "1") {
|
||
fpRemoveVertexBtn.dataset.bound = "1";
|
||
fpRemoveVertexBtn.addEventListener("click", () => {
|
||
if (removeSelectedFootprintVertex()) persistLayoutDebounced();
|
||
});
|
||
}
|
||
[
|
||
fpLengthMEl,
|
||
fpWidthMEl,
|
||
fpRadiusMEl,
|
||
fpCircleSegmentsEl,
|
||
fpPolyRadiusMEl,
|
||
fpPolySidesEl,
|
||
].forEach((node) => {
|
||
if (!node || node.dataset.bound === "1") return;
|
||
node.dataset.bound = "1";
|
||
node.addEventListener("change", () => {
|
||
readFootprintParamsFromDOM();
|
||
if (!isCustomFootprintShape()) applyFootprintPreset();
|
||
});
|
||
});
|
||
}
|
||
|
||
function ensureFootprint() {
|
||
ensureFootprintSchema();
|
||
if (!Array.isArray(state.layout.robot.footprint) || state.layout.robot.footprint.length < 3) {
|
||
state.layout.robot.footprint = [
|
||
{ x: 120, y: 80 },
|
||
{ x: 120, y: -80 },
|
||
{ x: -90, y: -80 },
|
||
{ x: -90, y: 80 },
|
||
];
|
||
if (!state.layout.robot.footprint_shape) state.layout.robot.footprint_shape = "rectangle";
|
||
state.layout.robot.footprint_params = inferFootprintParamsFromPoints(state.layout.robot.footprint);
|
||
}
|
||
}
|
||
|
||
function getFootprintAbsPoints() {
|
||
ensureFootprint();
|
||
return state.layout.robot.footprint
|
||
.map((p) => robotToAbs(Number(p.x || 0), Number(p.y || 0)))
|
||
.filter((p) => Number.isFinite(p.x) && Number.isFinite(p.y));
|
||
}
|
||
|
||
const FOOTPRINT_MIN_VERTICES = 3;
|
||
const FOOTPRINT_MAX_VERTICES = 64;
|
||
|
||
function hitTestFootprintVertex(x, y, radius = 10) {
|
||
radius = radius / Math.max(state.view.scale, 0.12);
|
||
const pts = getFootprintAbsPoints();
|
||
const r2 = radius * radius;
|
||
for (let i = pts.length - 1; i >= 0; i--) {
|
||
const dx = x - pts[i].x;
|
||
const dy = y - pts[i].y;
|
||
if (dx * dx + dy * dy <= r2) return i;
|
||
}
|
||
return null;
|
||
}
|
||
|
||
function distPointToSegment(px, py, x1, y1, x2, y2) {
|
||
const dx = x2 - x1;
|
||
const dy = y2 - y1;
|
||
const len2 = dx * dx + dy * dy;
|
||
if (len2 < 1e-6) {
|
||
return { dist: Math.hypot(px - x1, py - y1), qx: x1, qy: y1 };
|
||
}
|
||
const t = clamp(((px - x1) * dx + (py - y1) * dy) / len2, 0, 1);
|
||
const qx = x1 + t * dx;
|
||
const qy = y1 + t * dy;
|
||
return { dist: Math.hypot(px - qx, py - qy), qx, qy };
|
||
}
|
||
|
||
function hitTestFootprintEdge(x, y, threshold = 12) {
|
||
threshold = threshold / Math.max(state.view.scale, 0.12);
|
||
const pts = getFootprintAbsPoints();
|
||
if (pts.length < 2) return null;
|
||
let best = null;
|
||
for (let i = 0; i < pts.length; i++) {
|
||
const j = (i + 1) % pts.length;
|
||
const seg = distPointToSegment(x, y, pts[i].x, pts[i].y, pts[j].x, pts[j].y);
|
||
if (seg.dist <= threshold && (!best || seg.dist < best.dist)) {
|
||
const rel = absToRobot(seg.qx, seg.qy);
|
||
best = { insertAfter: i, rel, dist: seg.dist };
|
||
}
|
||
}
|
||
return best;
|
||
}
|
||
|
||
function updateFootprintVertexUI() {
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
const n = fp.length;
|
||
if (fpVertexCountEl) fpVertexCountEl.textContent = String(n);
|
||
const sel = state.selectedFootprintVertex;
|
||
if (fpSelectedVertexTextEl) {
|
||
fpSelectedVertexTextEl.textContent =
|
||
sel !== null && sel >= 0 && sel < n ? `Đã chọn: đỉnh #${sel}` : "Chưa chọn đỉnh";
|
||
}
|
||
if (fpRemoveVertexBtn) {
|
||
fpRemoveVertexBtn.disabled = sel === null || n <= FOOTPRINT_MIN_VERTICES;
|
||
}
|
||
if (fpAddVertexBtn) fpAddVertexBtn.disabled = n >= FOOTPRINT_MAX_VERTICES;
|
||
}
|
||
|
||
function insertFootprintVertex(insertAfter, rel) {
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
if (fp.length >= FOOTPRINT_MAX_VERTICES) {
|
||
setStatus(`Tối đa ${FOOTPRINT_MAX_VERTICES} đỉnh`);
|
||
return null;
|
||
}
|
||
const idx = insertAfter + 1;
|
||
fp.splice(idx, 0, { x: rel.x, y: rel.y });
|
||
state.layout.robot.footprint_shape = "custom";
|
||
if (footprintShapeEl) footprintShapeEl.value = "custom";
|
||
state.selectedFootprintVertex = idx;
|
||
updateFootprintPresetPanelVisibility();
|
||
updateFootprintVertexUI();
|
||
validateDiff();
|
||
renderCanvas();
|
||
return idx;
|
||
}
|
||
|
||
function addFootprintVertexAt(relX, relY) {
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
if (fp.length >= FOOTPRINT_MAX_VERTICES) {
|
||
setStatus(`Tối đa ${FOOTPRINT_MAX_VERTICES} đỉnh`);
|
||
return null;
|
||
}
|
||
fp.push({ x: relX, y: relY });
|
||
state.layout.robot.footprint_shape = "custom";
|
||
if (footprintShapeEl) footprintShapeEl.value = "custom";
|
||
state.selectedFootprintVertex = fp.length - 1;
|
||
updateFootprintPresetPanelVisibility();
|
||
updateFootprintVertexUI();
|
||
validateDiff();
|
||
renderCanvas();
|
||
return fp.length - 1;
|
||
}
|
||
|
||
function addFootprintVertexFromUI() {
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
const sel = state.selectedFootprintVertex;
|
||
if (sel !== null && sel >= 0 && sel < fp.length) {
|
||
const a = fp[sel];
|
||
const b = fp[(sel + 1) % fp.length];
|
||
const rel = {
|
||
x: (Number(a.x) + Number(b.x)) / 2,
|
||
y: (Number(a.y) + Number(b.y)) / 2,
|
||
};
|
||
insertFootprintVertex(sel, rel);
|
||
setStatus(`Đã thêm đỉnh giữa #${sel} và #${(sel + 1) % fp.length}`);
|
||
return;
|
||
}
|
||
let cx = 0;
|
||
let cy = 0;
|
||
fp.forEach((p) => {
|
||
cx += Number(p.x);
|
||
cy += Number(p.y);
|
||
});
|
||
cx /= fp.length || 1;
|
||
cy /= fp.length || 1;
|
||
const scale = getScaleMPerPx();
|
||
addFootprintVertexAt(cx + 40 * scale, cy);
|
||
setStatus("Đã thêm đỉnh mới");
|
||
}
|
||
|
||
function addFootprintVertexFromCanvas(absX, absY) {
|
||
const edge = hitTestFootprintEdge(absX, absY, 14);
|
||
if (edge) {
|
||
const idx = insertFootprintVertex(edge.insertAfter, edge.rel);
|
||
if (idx !== null) setStatus(`Đã chèn đỉnh #${idx} trên cạnh`);
|
||
return;
|
||
}
|
||
const rel = absToRobot(absX, absY);
|
||
const idx = addFootprintVertexAt(rel.x, rel.y);
|
||
if (idx !== null) setStatus(`Đã thêm đỉnh #${idx} — click cạnh để chèn giữa 2 đỉnh`);
|
||
}
|
||
|
||
function removeSelectedFootprintVertex() {
|
||
if (state.selectedFootprintVertex === null) {
|
||
setStatus("Chọn đỉnh cần xóa (click trên canvas)");
|
||
return false;
|
||
}
|
||
ensureFootprint();
|
||
const fp = state.layout.robot.footprint;
|
||
if (fp.length <= FOOTPRINT_MIN_VERTICES) {
|
||
setStatus(`Footprint cần ít nhất ${FOOTPRINT_MIN_VERTICES} đỉnh`);
|
||
return false;
|
||
}
|
||
const removed = state.selectedFootprintVertex;
|
||
fp.splice(removed, 1);
|
||
state.selectedFootprintVertex = null;
|
||
updateFootprintVertexUI();
|
||
validateDiff();
|
||
renderCanvas();
|
||
setStatus(`Đã xóa đỉnh #${removed}`);
|
||
return true;
|
||
}
|
||
|
||
function setLidarListPanelCollapsed(collapsed) {
|
||
state.lidarListPanelCollapsed = collapsed;
|
||
lidarListCard.classList.toggle("collapsed", collapsed);
|
||
lidarListCardToggle.setAttribute("aria-expanded", String(!collapsed));
|
||
try {
|
||
localStorage.setItem("lidarListPanelCollapsed", collapsed ? "1" : "0");
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
}
|
||
|
||
function setRobotModelPanelCollapsed(collapsed) {
|
||
state.robotModelPanelCollapsed = collapsed;
|
||
robotModelCard.classList.toggle("collapsed", collapsed);
|
||
robotModelCardToggle.setAttribute("aria-expanded", String(!collapsed));
|
||
try {
|
||
localStorage.setItem("robotModelPanelCollapsed", collapsed ? "1" : "0");
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
}
|
||
|
||
function toggleLidarItemCollapsed(id) {
|
||
state.lidarItemCollapsed[id] = !state.lidarItemCollapsed[id];
|
||
const item = listEl.querySelector(`.item[data-lidar-id="${id}"]`);
|
||
if (item) {
|
||
item.classList.toggle("collapsed", !!state.lidarItemCollapsed[id]);
|
||
const btn = item.querySelector('[data-action="toggle-item"]');
|
||
if (btn) btn.setAttribute("aria-expanded", String(!state.lidarItemCollapsed[id]));
|
||
}
|
||
}
|
||
|
||
let persistLayoutTimer = null;
|
||
|
||
function markLayoutDirty() {
|
||
state.layoutDirty = true;
|
||
updateLayoutActiveHint();
|
||
}
|
||
|
||
function clearLayoutDirty() {
|
||
state.layoutDirty = false;
|
||
updateLayoutActiveHint();
|
||
}
|
||
|
||
function updateLayoutActiveHint() {
|
||
if (!layoutActiveHintEl) return;
|
||
const name = state.activeLayoutName || "—";
|
||
const dirty = state.layoutDirty ? " • chưa lưu" : "";
|
||
layoutActiveHintEl.textContent = `Đang chỉnh: ${name}${dirty}`;
|
||
}
|
||
|
||
function renderLayoutSelect() {
|
||
if (!layoutSelectEl) return;
|
||
const options = (state.layoutCatalog || [])
|
||
.map(
|
||
(p) =>
|
||
`<option value="${escapeHtml(p.id)}"${p.id === state.activeLayoutId ? " selected" : ""}>${escapeHtml(p.name)}</option>`,
|
||
)
|
||
.join("");
|
||
layoutSelectEl.innerHTML = options || '<option value="">—</option>';
|
||
updateLayoutActiveHint();
|
||
}
|
||
|
||
async function persistLayoutNow() {
|
||
if (state.activeLayoutId) {
|
||
await api(`/api/layouts/${state.activeLayoutId}`, {
|
||
method: "PUT",
|
||
body: JSON.stringify({ layout: state.layout, lidars: state.lidars, imus: state.imus }),
|
||
});
|
||
} else {
|
||
await api("/api/layout", { method: "PUT", body: JSON.stringify(state.layout) });
|
||
}
|
||
clearLayoutDirty();
|
||
}
|
||
|
||
function persistLayoutDebounced() {
|
||
clearTimeout(persistLayoutTimer);
|
||
persistLayoutTimer = setTimeout(async () => {
|
||
try {
|
||
await persistLayoutNow();
|
||
} catch (e) {
|
||
setStatus(`Lỗi lưu layout: ${e.message}`);
|
||
}
|
||
}, 450);
|
||
}
|
||
|
||
async function saveCurrentLayout() {
|
||
if ((state.layout.robot.model || "diff") === "bicycle") applyBicycleFormToState();
|
||
else applyDiffFormToState();
|
||
await persistLayoutNow();
|
||
}
|
||
|
||
async function createLayoutFromUI() {
|
||
const name = layoutNewNameEl?.value?.trim() || "";
|
||
if (!name) {
|
||
setStatus("Nhập tên layout mới");
|
||
return;
|
||
}
|
||
const clone = !!layoutCloneCurrentEl?.checked;
|
||
await api("/api/layouts", {
|
||
method: "POST",
|
||
body: JSON.stringify({ name, clone }),
|
||
});
|
||
if (layoutNewNameEl) layoutNewNameEl.value = "";
|
||
if (layoutCloneCurrentEl) layoutCloneCurrentEl.checked = false;
|
||
state.viewInitialized = false;
|
||
await loadAll();
|
||
setStatus(`Đã tạo layout «${name}»`);
|
||
}
|
||
|
||
async function switchToLayout(id) {
|
||
if (!id || id === state.activeLayoutId) return;
|
||
if (state.layoutDirty) {
|
||
const ok = window.confirm(
|
||
"Layout hiện tại có thay đổi chưa lưu. Chuyển layout sẽ không lưu các thay đổi đó. Tiếp tục?",
|
||
);
|
||
if (!ok) {
|
||
renderLayoutSelect();
|
||
return;
|
||
}
|
||
}
|
||
await api(`/api/layouts/${id}/activate`, { method: "POST" });
|
||
state.viewInitialized = false;
|
||
await loadAll();
|
||
setStatus("Đã chuyển layout");
|
||
}
|
||
|
||
async function deleteActiveLayoutFromUI() {
|
||
if (!state.activeLayoutId) return;
|
||
if ((state.layoutCatalog || []).length <= 1) {
|
||
setStatus("Không thể xóa layout cuối cùng");
|
||
return;
|
||
}
|
||
const name = state.activeLayoutName || state.activeLayoutId;
|
||
if (!window.confirm(`Xóa layout «${name}»? Hành động không hoàn tác.`)) return;
|
||
await api(`/api/layouts/${state.activeLayoutId}`, { method: "DELETE" });
|
||
state.viewInitialized = false;
|
||
await loadAll();
|
||
setStatus(`Đã xóa layout «${name}»`);
|
||
}
|
||
|
||
function initLayoutManagerEvents() {
|
||
if (layoutSelectEl && layoutSelectEl.dataset.bound !== "1") {
|
||
layoutSelectEl.dataset.bound = "1";
|
||
layoutSelectEl.addEventListener("change", () => {
|
||
void switchToLayout(layoutSelectEl.value);
|
||
});
|
||
}
|
||
if (layoutCreateBtn && layoutCreateBtn.dataset.bound !== "1") {
|
||
layoutCreateBtn.dataset.bound = "1";
|
||
layoutCreateBtn.addEventListener("click", () => {
|
||
void createLayoutFromUI().catch((e) => setStatus(`Lỗi: ${e.message}`));
|
||
});
|
||
}
|
||
if (layoutDeleteBtn && layoutDeleteBtn.dataset.bound !== "1") {
|
||
layoutDeleteBtn.dataset.bound = "1";
|
||
layoutDeleteBtn.addEventListener("click", () => {
|
||
void deleteActiveLayoutFromUI().catch((e) => setStatus(`Lỗi: ${e.message}`));
|
||
});
|
||
}
|
||
}
|
||
|
||
/** Sync list row meta + X/Y/θ inputs from robot-frame pose (e.g. after canvas drag). */
|
||
function updateLidarItemPoseUI(id) {
|
||
const item = listEl.querySelector(`.item[data-lidar-id="${id}"]`);
|
||
if (!item) return;
|
||
const l = state.lidars.find((x) => x.id === id);
|
||
const pose = state.layout.lidarPoses[id];
|
||
const meta = item.querySelector(".itemMeta");
|
||
if (!pose) {
|
||
if (meta) meta.textContent = l ? `${l.ip}:${l.port} • chưa đặt pose` : "chưa đặt pose";
|
||
return;
|
||
}
|
||
|
||
const x = Number(pose.x || 0);
|
||
const y = Number(pose.y || 0);
|
||
const th = Number(pose.theta_deg || 0);
|
||
const posTxt = `theo tâm robot: x=${x.toFixed(0)}, y=${y.toFixed(0)}, θ=${th.toFixed(0)}°`;
|
||
if (meta && l) meta.textContent = `${l.ip}:${l.port} • ${posTxt}`;
|
||
else if (meta) meta.textContent = posTxt;
|
||
|
||
const active = document.activeElement;
|
||
const xIn = item.querySelector('input.poseInput[data-action="x"]');
|
||
const yIn = item.querySelector('input.poseInput[data-action="y"]');
|
||
const tIn = item.querySelector('input.poseInput[data-action="theta"]');
|
||
if (xIn && active !== xIn) xIn.value = x.toFixed(0);
|
||
if (yIn && active !== yIn) yIn.value = y.toFixed(0);
|
||
if (tIn && active !== tIn) tIn.value = String(Math.round(th));
|
||
}
|
||
|
||
function refreshLidarSelectionUI() {
|
||
listEl.querySelectorAll(".item[data-lidar-id]").forEach((item) => {
|
||
const id = item.dataset.lidarId;
|
||
const l = state.lidars.find((x) => x.id === id);
|
||
if (!l) return;
|
||
const nameEl = item.querySelector(".itemName");
|
||
if (!nameEl) return;
|
||
const selected = state.selectedId === id ? `<span class="pill">selected</span>` : "";
|
||
nameEl.innerHTML = `${escapeHtml(l.name)} ${selected}`;
|
||
});
|
||
}
|
||
|
||
function onLidarPoseInputChange(id, action, value) {
|
||
ensureDefaultPose(id, 0);
|
||
const pose = state.layout.lidarPoses[id];
|
||
if (!pose) return;
|
||
|
||
if (action === "theta") {
|
||
pose.theta_deg = clamp(Number(value), -180, 180);
|
||
} else if (action === "x") {
|
||
pose.x = Number(value);
|
||
} else if (action === "y") {
|
||
pose.y = Number(value);
|
||
}
|
||
|
||
if (state.selectedId === id) setSelectedRelText();
|
||
updateLidarItemPoseUI(id);
|
||
renderCanvas();
|
||
persistLayoutDebounced();
|
||
}
|
||
|
||
function initLidarListEvents() {
|
||
listEl.addEventListener("click", async (evt) => {
|
||
const btn = evt.target.closest("button[data-action][data-id]");
|
||
if (!btn) return;
|
||
const action = btn.dataset.action;
|
||
const id = btn.dataset.id;
|
||
if (!action || !id) return;
|
||
|
||
if (action === "toggle-item") {
|
||
evt.stopPropagation();
|
||
toggleLidarItemCollapsed(id);
|
||
return;
|
||
}
|
||
if (action === "select") {
|
||
selectLidarOnCanvas(id);
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
if (action === "delete") {
|
||
if (!confirm("Xóa LiDAR này?")) return;
|
||
try {
|
||
await api(`/api/lidars/${id}`, { method: "DELETE" });
|
||
state.lidars = state.lidars.filter((l) => l.id !== id);
|
||
if (state.layout?.lidarPoses) delete state.layout.lidarPoses[id];
|
||
delete state.lidarItemCollapsed[id];
|
||
if (state.selectedId === id) clearCanvasSelection();
|
||
setSelectedRelText();
|
||
renderList();
|
||
renderCanvas();
|
||
setStatus("Đã xóa LiDAR");
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
}
|
||
}
|
||
});
|
||
|
||
listEl.addEventListener("change", (evt) => {
|
||
const input = evt.target.closest("input.poseInput[data-action][data-id]");
|
||
if (!input) return;
|
||
onLidarPoseInputChange(input.dataset.id, input.dataset.action, input.value);
|
||
if (input.dataset.action === "theta") {
|
||
input.value = String(state.layout.lidarPoses[input.dataset.id].theta_deg);
|
||
}
|
||
});
|
||
}
|
||
|
||
function toggleImuItemCollapsed(id) {
|
||
state.imuItemCollapsed[id] = !state.imuItemCollapsed[id];
|
||
renderImuList();
|
||
}
|
||
|
||
function onImuPoseInputChange(id, action, value) {
|
||
ensureDefaultImuPose(id, 0);
|
||
const pose = state.layout.imuPoses[id];
|
||
if (!pose) return;
|
||
if (action === "yaw") pose.yaw_deg = clamp(Number(value), -180, 180);
|
||
else if (action === "x") pose.x = Number(value);
|
||
else if (action === "y") pose.y = Number(value);
|
||
else if (action === "z") pose.z = clamp(Number(value), -5, 5);
|
||
if (state.selectedImuId === id) setSelectedRelText();
|
||
updateImuItemPoseUI(id);
|
||
renderCanvas();
|
||
persistLayoutDebounced();
|
||
}
|
||
|
||
function initImuListEvents() {
|
||
if (!imuListEl || imuListEl.dataset.bound === "1") return;
|
||
imuListEl.dataset.bound = "1";
|
||
|
||
imuListEl.addEventListener("click", async (evt) => {
|
||
const btn = evt.target.closest("button[data-action][data-id]");
|
||
if (!btn) return;
|
||
const action = btn.dataset.action;
|
||
const id = btn.dataset.id;
|
||
if (!action || !id) return;
|
||
|
||
if (action === "toggle-item") {
|
||
evt.stopPropagation();
|
||
toggleImuItemCollapsed(id);
|
||
return;
|
||
}
|
||
if (action === "select") {
|
||
selectImuOnCanvas(id);
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
if (action === "delete") {
|
||
if (!confirm("Xóa IMU này?")) return;
|
||
try {
|
||
await api(`/api/imus/${id}`, { method: "DELETE" });
|
||
state.imus = state.imus.filter((im) => im.id !== id);
|
||
if (state.layout?.imuPoses) delete state.layout.imuPoses[id];
|
||
delete state.imuItemCollapsed[id];
|
||
if (state.selectedImuId === id) clearCanvasSelection();
|
||
setSelectedRelText();
|
||
renderImuList();
|
||
renderCanvas();
|
||
setStatus("Đã xóa IMU");
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
}
|
||
}
|
||
});
|
||
|
||
imuListEl.addEventListener("change", (evt) => {
|
||
const input = evt.target.closest("input.poseInput[data-action][data-id]");
|
||
if (!input) return;
|
||
onImuPoseInputChange(input.dataset.id, input.dataset.action, input.value);
|
||
});
|
||
}
|
||
|
||
function setImuFormHint(msg) {
|
||
if (!imuFormHintEl) return;
|
||
if (!msg) {
|
||
imuFormHintEl.hidden = true;
|
||
imuFormHintEl.textContent = "";
|
||
return;
|
||
}
|
||
imuFormHintEl.hidden = false;
|
||
imuFormHintEl.textContent = msg;
|
||
}
|
||
|
||
let imuFormBusy = false;
|
||
|
||
async function submitAddImu() {
|
||
if (imuFormBusy) return;
|
||
imuFormBusy = true;
|
||
const addBtn = el("addImuBtn");
|
||
if (addBtn) addBtn.disabled = true;
|
||
|
||
const name = String(el("imuName")?.value || "").trim();
|
||
const frame_id = String(el("imuFrameId")?.value || "").trim();
|
||
const topic = String(el("imuTopic")?.value || "").trim();
|
||
const source = el("imuSource")?.value || "external";
|
||
const rate_hz = clamp(Number(el("imuRateHz")?.value), 1, 1000);
|
||
const enabled = !!el("imuEnabled")?.checked;
|
||
|
||
if (!name || !frame_id || !topic) {
|
||
setImuFormHint("Nhập đủ tên, frame_id và topic.");
|
||
setStatus("Thiếu thông tin IMU");
|
||
imuFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
return;
|
||
}
|
||
|
||
const dup = findDuplicateImuFrame(frame_id);
|
||
if (dup) {
|
||
const msg = `IMU trùng frame_id «${frame_id}» (${dup.name}).`;
|
||
setImuFormHint(msg);
|
||
setStatus(msg);
|
||
imuFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
return;
|
||
}
|
||
|
||
setImuFormHint("");
|
||
try {
|
||
const created = await api("/api/imus", {
|
||
method: "POST",
|
||
body: JSON.stringify({ name, frame_id, topic, source, rate_hz, enabled }),
|
||
});
|
||
state.imus.push(created);
|
||
ensureDefaultImuPose(created.id, state.imus.length - 1);
|
||
reconcileImuPoses();
|
||
await persistLayoutNow();
|
||
if (el("imuName")) el("imuName").value = "";
|
||
if (el("imuFrameId")) el("imuFrameId").value = "";
|
||
renderImuList();
|
||
renderCanvas();
|
||
setStatus("Đã thêm IMU");
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
} finally {
|
||
imuFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
}
|
||
}
|
||
|
||
function initImuForm() {
|
||
const form = el("imuForm");
|
||
if (!form || form.dataset.bound === "1") return;
|
||
form.dataset.bound = "1";
|
||
form.addEventListener("submit", (evt) => {
|
||
evt.preventDefault();
|
||
void submitAddImu();
|
||
});
|
||
el("addImuBtn")?.addEventListener("click", (evt) => {
|
||
evt.preventDefault();
|
||
void submitAddImu();
|
||
});
|
||
["imuName", "imuFrameId", "imuTopic"].forEach((id) => {
|
||
el(id)?.addEventListener("input", () => setImuFormHint(""));
|
||
});
|
||
}
|
||
|
||
function setImuListPanelCollapsed(collapsed) {
|
||
state.imuListPanelCollapsed = collapsed;
|
||
if (imuListCard) imuListCard.classList.toggle("collapsed", collapsed);
|
||
if (imuListCardToggle) imuListCardToggle.setAttribute("aria-expanded", String(!collapsed));
|
||
try {
|
||
localStorage.setItem("imuListPanelCollapsed", collapsed ? "1" : "0");
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
}
|
||
|
||
function initImuListPanelCollapse() {
|
||
if (!imuListCardToggle) return;
|
||
try {
|
||
const saved = localStorage.getItem("imuListPanelCollapsed");
|
||
if (saved === "1") setImuListPanelCollapsed(true);
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
const toggle = () => setImuListPanelCollapsed(!state.imuListPanelCollapsed);
|
||
imuListCardToggle.addEventListener("click", toggle);
|
||
imuListCardToggle.addEventListener("keydown", (evt) => {
|
||
if (evt.key === "Enter" || evt.key === " ") {
|
||
evt.preventDefault();
|
||
toggle();
|
||
}
|
||
});
|
||
}
|
||
|
||
function setEditFootprintMode(on) {
|
||
state.editFootprint = on;
|
||
editFootprintBtn.classList.toggle("active", on);
|
||
canvas.classList.toggle("edit-footprint", on);
|
||
if (footprintEditHint) footprintEditHint.hidden = !on;
|
||
if (on) {
|
||
updateFootprintEditHint();
|
||
updateFootprintVertexUI();
|
||
}
|
||
if (!on) {
|
||
state.selectedFootprintVertex = null;
|
||
state.draggingFootprint = null;
|
||
updateFootprintVertexUI();
|
||
}
|
||
renderCanvas();
|
||
}
|
||
|
||
function setSelectedRelText() {
|
||
if (state.selectedImuId) {
|
||
const pose = getImuPoseAbs(state.selectedImuId);
|
||
if (!pose) {
|
||
selectedRelText.textContent = "—";
|
||
return;
|
||
}
|
||
const yaw = Number(pose.yaw_deg || 0);
|
||
const z = Number(pose.z || 0);
|
||
selectedRelText.textContent = `(x=${Number(pose.x || 0).toFixed(0)}, y=${Number(pose.y || 0).toFixed(0)}, z=${z.toFixed(2)}, ψ=${yaw.toFixed(0)}°)`;
|
||
return;
|
||
}
|
||
if (!state.selectedId) {
|
||
selectedRelText.textContent = "—";
|
||
return;
|
||
}
|
||
const pose = getLidarPoseAbs(state.selectedId);
|
||
if (!pose) {
|
||
selectedRelText.textContent = "—";
|
||
return;
|
||
}
|
||
const th = Number(pose.theta_deg || 0);
|
||
selectedRelText.textContent = `(x=${Number(pose.x || 0).toFixed(0)}, y=${Number(pose.y || 0).toFixed(0)}, θ=${th.toFixed(0)}°)`;
|
||
}
|
||
|
||
function renderList() {
|
||
if (!state.lidars.length) {
|
||
listEl.innerHTML = `<div class="item"><div class="itemName">Chưa có LiDAR</div><div class="itemMeta">Hãy thêm LiDAR ở form phía trên.</div></div>`;
|
||
return;
|
||
}
|
||
|
||
listEl.innerHTML = state.lidars
|
||
.map((l, idx) => {
|
||
ensureDefaultPose(l.id, idx);
|
||
const pose = getLidarPoseAbs(l.id);
|
||
let posTxt = "chưa đặt pose";
|
||
let xRobot = 0;
|
||
let yRobot = 0;
|
||
let thetaDeg = 0;
|
||
if (pose) {
|
||
xRobot = Number(pose.x || 0);
|
||
yRobot = Number(pose.y || 0);
|
||
thetaDeg = Number(pose.theta_deg || 0);
|
||
posTxt = `theo tâm robot: x=${xRobot.toFixed(0)}, y=${yRobot.toFixed(0)}, θ=${thetaDeg.toFixed(0)}°`;
|
||
}
|
||
const selected = state.selectedId === l.id ? `<span class="pill">selected</span>` : "";
|
||
const itemCollapsed = !!state.lidarItemCollapsed[l.id];
|
||
return `
|
||
<div class="item ${itemCollapsed ? "collapsed" : ""}" data-lidar-id="${l.id}">
|
||
<div class="itemTop">
|
||
<div class="itemTopRow">
|
||
<button
|
||
type="button"
|
||
class="itemToggle"
|
||
data-action="toggle-item"
|
||
data-id="${l.id}"
|
||
aria-expanded="${!itemCollapsed}"
|
||
aria-label="Đóng/mở chi tiết ${escapeHtml(l.name)}"
|
||
></button>
|
||
<div class="itemMain">
|
||
<div class="itemName">${escapeHtml(l.name)} ${selected}</div>
|
||
<div class="itemMeta">${escapeHtml(l.ip)}:${l.port} • ${posTxt}</div>
|
||
</div>
|
||
</div>
|
||
<div class="itemBtns">
|
||
<button class="btn subtle" data-action="select" data-id="${l.id}">Chọn</button>
|
||
<button class="btn subtle danger" data-action="delete" data-id="${l.id}">Xóa</button>
|
||
</div>
|
||
</div>
|
||
<div class="itemBody">
|
||
<div class="poseRow">
|
||
<div class="poseField">
|
||
<span class="poseLabel">X</span>
|
||
<input class="poseInput" data-action="x" data-id="${l.id}" type="number" step="1" value="${xRobot.toFixed(0)}" />
|
||
</div>
|
||
<div class="poseField">
|
||
<span class="poseLabel">Y</span>
|
||
<input class="poseInput" data-action="y" data-id="${l.id}" type="number" step="1" value="${yRobot.toFixed(0)}" />
|
||
</div>
|
||
<div class="poseField">
|
||
<span class="poseLabel">θ</span>
|
||
<input class="poseInput" data-action="theta" data-id="${l.id}" type="number" min="-180" max="180" step="1" value="${thetaDeg}" />
|
||
</div>
|
||
</div>
|
||
</div>
|
||
</div>`;
|
||
})
|
||
.join("");
|
||
}
|
||
|
||
function refreshImuSelectionUI() {
|
||
if (!imuListEl) return;
|
||
imuListEl.querySelectorAll(".item[data-imu-id]").forEach((item) => {
|
||
const id = item.dataset.imuId;
|
||
const im = state.imus.find((x) => x.id === id);
|
||
if (!im) return;
|
||
const nameEl = item.querySelector(".itemName");
|
||
if (!nameEl) return;
|
||
const selected = state.selectedImuId === id ? `<span class="pill">selected</span>` : "";
|
||
nameEl.innerHTML = `${escapeHtml(im.name)} ${selected}`;
|
||
});
|
||
}
|
||
|
||
function updateImuItemPoseUI(id) {
|
||
const item = imuListEl?.querySelector(`.item[data-imu-id="${id}"]`);
|
||
if (!item) return;
|
||
const im = state.imus.find((x) => x.id === id);
|
||
const pose = state.layout.imuPoses?.[id];
|
||
const meta = item.querySelector(".itemMeta");
|
||
if (!pose) {
|
||
if (meta && im) meta.textContent = `${im.frame_id} • chưa đặt pose`;
|
||
return;
|
||
}
|
||
const x = Number(pose.x || 0);
|
||
const y = Number(pose.y || 0);
|
||
const z = Number(pose.z || 0);
|
||
const yaw = Number(pose.yaw_deg || 0);
|
||
const posTxt = `x=${x.toFixed(0)}, y=${y.toFixed(0)}, z=${z.toFixed(2)}, ψ=${yaw.toFixed(0)}°`;
|
||
if (meta && im) meta.textContent = `${im.frame_id} • ${posTxt}`;
|
||
const active = document.activeElement;
|
||
const xIn = item.querySelector('input.poseInput[data-action="x"]');
|
||
const yIn = item.querySelector('input.poseInput[data-action="y"]');
|
||
const zIn = item.querySelector('input.poseInput[data-action="z"]');
|
||
const yawIn = item.querySelector('input.poseInput[data-action="yaw"]');
|
||
if (xIn && active !== xIn) xIn.value = x.toFixed(0);
|
||
if (yIn && active !== yIn) yIn.value = y.toFixed(0);
|
||
if (zIn && active !== zIn) zIn.value = z.toFixed(2);
|
||
if (yawIn && active !== yawIn) yawIn.value = String(Math.round(yaw));
|
||
}
|
||
|
||
function renderImuList() {
|
||
if (!imuListEl) return;
|
||
if (!state.imus.length) {
|
||
imuListEl.innerHTML = `<div class="item"><div class="itemName">Chưa có IMU</div><div class="itemMeta">Thêm IMU ở form phía trên.</div></div>`;
|
||
return;
|
||
}
|
||
|
||
imuListEl.innerHTML = state.imus
|
||
.map((im, idx) => {
|
||
ensureDefaultImuPose(im.id, idx);
|
||
const pose = getImuPoseAbs(im.id);
|
||
let posTxt = "chưa đặt pose";
|
||
let xRobot = 0;
|
||
let yRobot = 0;
|
||
let zM = 0.1;
|
||
let yawDeg = 0;
|
||
if (pose) {
|
||
xRobot = Number(pose.x || 0);
|
||
yRobot = Number(pose.y || 0);
|
||
zM = Number(pose.z ?? 0.1);
|
||
yawDeg = Number(pose.yaw_deg || 0);
|
||
posTxt = `x=${xRobot.toFixed(0)}, y=${yRobot.toFixed(0)}, z=${zM.toFixed(2)}, ψ=${yawDeg.toFixed(0)}°`;
|
||
}
|
||
const selected = state.selectedImuId === im.id ? `<span class="pill">selected</span>` : "";
|
||
const itemCollapsed = !!state.imuItemCollapsed[im.id];
|
||
const srcLabel =
|
||
im.source === "lidar_builtin" ? "LiDAR" : im.source === "onboard" ? "Onboard" : "ROS";
|
||
const enabledTxt = im.enabled === false ? " • tắt" : "";
|
||
return `
|
||
<div class="item imuItem ${itemCollapsed ? "collapsed" : ""}" data-imu-id="${im.id}">
|
||
<div class="itemTop">
|
||
<div class="itemTopRow">
|
||
<button
|
||
type="button"
|
||
class="itemToggle"
|
||
data-action="toggle-item"
|
||
data-id="${im.id}"
|
||
aria-expanded="${!itemCollapsed}"
|
||
aria-label="Đóng/mở chi tiết ${escapeHtml(im.name)}"
|
||
></button>
|
||
<div class="itemMain">
|
||
<div class="itemName">${escapeHtml(im.name)} ${selected}</div>
|
||
<div class="itemMeta">${escapeHtml(im.frame_id)} • ${escapeHtml(im.topic)} • ${srcLabel}${enabledTxt} • ${posTxt}</div>
|
||
</div>
|
||
</div>
|
||
<div class="itemBtns">
|
||
<button class="btn subtle" data-action="select" data-id="${im.id}">Chọn</button>
|
||
<button class="btn subtle danger" data-action="delete" data-id="${im.id}">Xóa</button>
|
||
</div>
|
||
</div>
|
||
<div class="itemBody">
|
||
<div class="poseRow">
|
||
<div class="poseField">
|
||
<span class="poseLabel">X</span>
|
||
<input class="poseInput" data-action="x" data-id="${im.id}" type="number" step="1" value="${xRobot.toFixed(0)}" />
|
||
</div>
|
||
<div class="poseField">
|
||
<span class="poseLabel">Y</span>
|
||
<input class="poseInput" data-action="y" data-id="${im.id}" type="number" step="1" value="${yRobot.toFixed(0)}" />
|
||
</div>
|
||
<div class="poseField">
|
||
<span class="poseLabel">Z</span>
|
||
<input class="poseInput" data-action="z" data-id="${im.id}" type="number" step="0.01" value="${zM.toFixed(2)}" />
|
||
</div>
|
||
<div class="poseField">
|
||
<span class="poseLabel">ψ</span>
|
||
<input class="poseInput" data-action="yaw" data-id="${im.id}" type="number" min="-180" max="180" step="1" value="${yawDeg}" />
|
||
</div>
|
||
</div>
|
||
</div>
|
||
</div>`;
|
||
})
|
||
.join("");
|
||
}
|
||
|
||
let lastCanvasW = 0;
|
||
let lastCanvasH = 0;
|
||
|
||
function syncCanvasSize() {
|
||
const wrap = canvas.parentElement;
|
||
if (!wrap) return false;
|
||
const w = Math.max(1, Math.floor(wrap.clientWidth));
|
||
const h = Math.max(1, Math.floor(wrap.clientHeight));
|
||
if (w === lastCanvasW && h === lastCanvasH) return false;
|
||
lastCanvasW = w;
|
||
lastCanvasH = h;
|
||
canvas.width = w;
|
||
canvas.height = h;
|
||
return true;
|
||
}
|
||
|
||
function fitViewToWorld() {
|
||
syncCanvasSize();
|
||
const worldW = state.layout.map.width;
|
||
const worldH = state.layout.map.height;
|
||
const pad = 36;
|
||
const scale = Math.min(
|
||
(canvas.width - pad * 2) / worldW,
|
||
(canvas.height - pad * 2) / worldH,
|
||
4,
|
||
);
|
||
state.view.scale = Math.max(0.15, scale);
|
||
state.view.panX = (canvas.width - worldW * state.view.scale) / 2;
|
||
state.view.panY = (canvas.height - worldH * state.view.scale) / 2;
|
||
}
|
||
|
||
function canvasScreenPoint(evt) {
|
||
const rect = canvas.getBoundingClientRect();
|
||
const scaleX = canvas.width / rect.width;
|
||
const scaleY = canvas.height / rect.height;
|
||
return {
|
||
x: (evt.clientX - rect.left) * scaleX,
|
||
y: (evt.clientY - rect.top) * scaleY,
|
||
};
|
||
}
|
||
|
||
function screenToWorld(sx, sy) {
|
||
return {
|
||
x: (sx - state.view.panX) / state.view.scale,
|
||
y: (sy - state.view.panY) / state.view.scale,
|
||
};
|
||
}
|
||
|
||
function canvasPoint(evt) {
|
||
const s = canvasScreenPoint(evt);
|
||
return screenToWorld(s.x, s.y);
|
||
}
|
||
|
||
function zoomAtScreen(sx, sy, factor) {
|
||
const before = screenToWorld(sx, sy);
|
||
const next = clamp(state.view.scale * factor, 0.12, 12);
|
||
state.view.scale = next;
|
||
state.view.panX = sx - before.x * next;
|
||
state.view.panY = sy - before.y * next;
|
||
}
|
||
|
||
function getVisibleWorldBounds() {
|
||
const pts = [
|
||
screenToWorld(0, 0),
|
||
screenToWorld(canvas.width, 0),
|
||
screenToWorld(canvas.width, canvas.height),
|
||
screenToWorld(0, canvas.height),
|
||
];
|
||
return {
|
||
minX: Math.min(...pts.map((p) => p.x)),
|
||
maxX: Math.max(...pts.map((p) => p.x)),
|
||
minY: Math.min(...pts.map((p) => p.y)),
|
||
maxY: Math.max(...pts.map((p) => p.y)),
|
||
};
|
||
}
|
||
|
||
/** Grid step in world px so lines stay ~40–56 px apart on screen when zooming. */
|
||
function getGridStep() {
|
||
const targetScreenPx = 48;
|
||
const raw = targetScreenPx / Math.max(state.view.scale, 0.01);
|
||
const pow = 10 ** Math.floor(Math.log10(raw));
|
||
const norm = raw / pow;
|
||
let nice = 10;
|
||
if (norm <= 1) nice = 1;
|
||
else if (norm <= 2) nice = 2;
|
||
else if (norm <= 5) nice = 5;
|
||
return nice * pow;
|
||
}
|
||
|
||
function drawGrid() {
|
||
const w = state.layout.map.width;
|
||
const h = state.layout.map.height;
|
||
const vis = getVisibleWorldBounds();
|
||
const pad = getGridStep() * 2;
|
||
const minX = vis.minX - pad;
|
||
const maxX = vis.maxX + pad;
|
||
const minY = vis.minY - pad;
|
||
const maxY = vis.maxY + pad;
|
||
|
||
ctx.fillStyle = "rgba(0,0,0,0.25)";
|
||
ctx.fillRect(minX, minY, maxX - minX, maxY - minY);
|
||
|
||
const major = getGridStep();
|
||
const minor = major / 5;
|
||
const lineW = 1 / Math.max(state.view.scale, 0.01);
|
||
|
||
function strokeLines(step, alpha) {
|
||
ctx.strokeStyle = `rgba(255,255,255,${alpha})`;
|
||
ctx.lineWidth = lineW;
|
||
const x0 = Math.floor(minX / step) * step;
|
||
const y0 = Math.floor(minY / step) * step;
|
||
for (let x = x0; x <= maxX; x += step) {
|
||
ctx.beginPath();
|
||
ctx.moveTo(x, minY);
|
||
ctx.lineTo(x, maxY);
|
||
ctx.stroke();
|
||
}
|
||
for (let y = y0; y <= maxY; y += step) {
|
||
ctx.beginPath();
|
||
ctx.moveTo(minX, y);
|
||
ctx.lineTo(maxX, y);
|
||
ctx.stroke();
|
||
}
|
||
}
|
||
|
||
if (minor >= 2 && state.view.scale >= 0.35) strokeLines(minor, 0.04);
|
||
strokeLines(major, 0.1);
|
||
|
||
ctx.strokeStyle = "rgba(255,255,255,0.22)";
|
||
ctx.lineWidth = lineW * 1.5;
|
||
ctx.strokeRect(0, 0, w, h);
|
||
}
|
||
|
||
function renderCanvas() {
|
||
syncCanvasSize();
|
||
const w = state.layout.map.width;
|
||
const h = state.layout.map.height;
|
||
|
||
ctx.setTransform(1, 0, 0, 1, 0, 0);
|
||
ctx.fillStyle = "#0b1220";
|
||
ctx.fillRect(0, 0, canvas.width, canvas.height);
|
||
|
||
ctx.save();
|
||
ctx.setTransform(state.view.scale, 0, 0, state.view.scale, state.view.panX, state.view.panY);
|
||
|
||
drawGrid();
|
||
|
||
const r = state.layout.robot;
|
||
robotCenterText.textContent = `(${r.x.toFixed(0)}, ${r.y.toFixed(0)})`;
|
||
|
||
// ROS-like axes icon at robot center (x red, y green), rotated by yaw_deg.
|
||
// Canvas coordinates: +x right, +y down.
|
||
// Interpret yaw_deg as ROS yaw (CCW around +Z). Convert to canvas angle by negating.
|
||
const yaw = yawCanvasRad();
|
||
const axisLen = 90;
|
||
const headLen = 12;
|
||
|
||
function arrow(fromX, fromY, toX, toY, color) {
|
||
ctx.strokeStyle = color;
|
||
ctx.fillStyle = color;
|
||
ctx.lineWidth = 3;
|
||
ctx.beginPath();
|
||
ctx.moveTo(fromX, fromY);
|
||
ctx.lineTo(toX, toY);
|
||
ctx.stroke();
|
||
|
||
const ang = Math.atan2(toY - fromY, toX - fromX);
|
||
ctx.beginPath();
|
||
ctx.moveTo(toX, toY);
|
||
ctx.lineTo(toX - headLen * Math.cos(ang - Math.PI / 7), toY - headLen * Math.sin(ang - Math.PI / 7));
|
||
ctx.lineTo(toX - headLen * Math.cos(ang + Math.PI / 7), toY - headLen * Math.sin(ang + Math.PI / 7));
|
||
ctx.closePath();
|
||
ctx.fill();
|
||
}
|
||
|
||
// Unit vectors for robot frame in canvas space (ROS): x forward, y left.
|
||
const ux = Math.cos(yaw);
|
||
const uy = Math.sin(yaw);
|
||
const vx = Math.cos(yaw - Math.PI / 2);
|
||
const vy = Math.sin(yaw - Math.PI / 2);
|
||
|
||
const xEnd = { x: r.x + ux * axisLen, y: r.y + uy * axisLen };
|
||
const yEnd = { x: r.x + vx * axisLen, y: r.y + vy * axisLen };
|
||
|
||
arrow(r.x, r.y, xEnd.x, xEnd.y, "rgba(255, 80, 80, 0.95)"); // X (red)
|
||
arrow(r.x, r.y, yEnd.x, yEnd.y, "rgba(110, 255, 140, 0.95)"); // Y (green)
|
||
|
||
ctx.font = "12px ui-sans-serif, system-ui";
|
||
ctx.fillStyle = "rgba(255, 140, 140, 0.95)";
|
||
ctx.fillText("x", xEnd.x + 6, xEnd.y + 4);
|
||
ctx.fillStyle = "rgba(160, 255, 190, 0.95)";
|
||
ctx.fillText("y", yEnd.x + 6, yEnd.y + 4);
|
||
|
||
// robot body (circle)
|
||
ctx.fillStyle = "rgba(128,237,153,0.18)";
|
||
ctx.strokeStyle = "rgba(128,237,153,0.55)";
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.arc(r.x, r.y, 38, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
|
||
// Robot footprint (ROS-like polygon), points are in robot frame (x forward, y left)
|
||
const pts = getFootprintAbsPoints();
|
||
if (pts.length >= 3) {
|
||
const editing = state.editFootprint;
|
||
ctx.fillStyle = editing ? "rgba(76, 201, 240, 0.14)" : "rgba(76, 201, 240, 0.08)";
|
||
ctx.strokeStyle = editing ? "rgba(76, 201, 240, 0.85)" : "rgba(76, 201, 240, 0.55)";
|
||
ctx.lineWidth = editing ? 2.5 : 2;
|
||
ctx.beginPath();
|
||
ctx.moveTo(pts[0].x, pts[0].y);
|
||
for (let i = 1; i < pts.length; i++) ctx.lineTo(pts[i].x, pts[i].y);
|
||
ctx.closePath();
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
|
||
if (editing && isCustomFootprintShape()) {
|
||
const fp = state.layout.robot.footprint;
|
||
pts.forEach((pt, i) => {
|
||
const selected = state.selectedFootprintVertex === i;
|
||
ctx.fillStyle = selected ? "rgba(255, 200, 80, 0.95)" : "rgba(76, 201, 240, 0.95)";
|
||
ctx.strokeStyle = "rgba(255,255,255,0.9)";
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.arc(pt.x, pt.y, selected ? 9 : 7, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
ctx.fillStyle = "rgba(232,238,252,0.85)";
|
||
ctx.font = "10px ui-sans-serif, system-ui";
|
||
const rel = fp[i];
|
||
ctx.fillText(`${i}`, pt.x + 10, pt.y - 6);
|
||
ctx.fillText(`(${Number(rel.x).toFixed(0)},${Number(rel.y).toFixed(0)})`, pt.x + 10, pt.y + 6);
|
||
});
|
||
}
|
||
}
|
||
|
||
// Differential drive wheels (when model = diff)
|
||
if ((state.layout.robot.model || "diff") === "diff") {
|
||
const dPx = getWheelDiameterPx();
|
||
const wheelR = Math.max(6, dPx / 2);
|
||
const wheelL = Math.max(18, dPx * 0.65);
|
||
const wheelW = Math.max(8, dPx * 0.22);
|
||
const wheels = getDiffWheelsForDraw();
|
||
const scale = state.layout.robot.diff.display.scale_m_per_px;
|
||
const centers = wheels.map((w) => {
|
||
const yRobot = Number(w.y_m ?? 0);
|
||
const yPx = yRobot / scale;
|
||
return { w, abs: robotToAbs(0, yPx) };
|
||
});
|
||
|
||
function roundRect(ctx2, x, y, w2, h2, r2) {
|
||
const rr = Math.min(r2, w2 / 2, h2 / 2);
|
||
ctx2.beginPath();
|
||
ctx2.moveTo(x + rr, y);
|
||
ctx2.arcTo(x + w2, y, x + w2, y + h2, rr);
|
||
ctx2.arcTo(x + w2, y + h2, x, y + h2, rr);
|
||
ctx2.arcTo(x, y + h2, x, y, rr);
|
||
ctx2.arcTo(x, y, x + w2, y, rr);
|
||
ctx2.closePath();
|
||
}
|
||
|
||
function drawWheel(center, w) {
|
||
const isLeft = w.side === "left" || w.id === "left";
|
||
const vendorShort = (w.motor?.vendor || "?").slice(0, 2).toUpperCase();
|
||
ctx.save();
|
||
ctx.translate(center.x, center.y);
|
||
ctx.rotate(yaw);
|
||
|
||
ctx.fillStyle = "rgba(255,255,255,0.10)";
|
||
ctx.strokeStyle = "rgba(255,255,255,0.28)";
|
||
ctx.lineWidth = 2;
|
||
roundRect(ctx, -wheelL / 2, -wheelW / 2, wheelL, wheelW, 6);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
|
||
ctx.fillStyle = "rgba(76,201,240,0.18)";
|
||
ctx.strokeStyle = "rgba(76,201,240,0.35)";
|
||
ctx.beginPath();
|
||
ctx.arc(0, 0, Math.min(10, wheelW / 2 - 2), 0, Math.PI * 2);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
|
||
ctx.fillStyle = "rgba(232,238,252,0.70)";
|
||
ctx.font = "10px ui-sans-serif, system-ui";
|
||
ctx.fillText(isLeft ? `L:${vendorShort}` : `R:${vendorShort}`, wheelL / 2 + 6, 3);
|
||
|
||
ctx.restore();
|
||
}
|
||
|
||
if (centers.length >= 2) {
|
||
ctx.strokeStyle = "rgba(160, 255, 190, 0.35)";
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.moveTo(centers[0].abs.x, centers[0].abs.y);
|
||
for (let i = 1; i < centers.length; i++) ctx.lineTo(centers[i].abs.x, centers[i].abs.y);
|
||
ctx.stroke();
|
||
}
|
||
centers.forEach(({ w, abs }) => drawWheel(abs, w));
|
||
}
|
||
|
||
// Kinematic bicycle (rear axle reference, front steer)
|
||
if ((state.layout.robot.model || "diff") === "bicycle") {
|
||
ensureBicycleSchema();
|
||
const b = state.layout.robot.bicycle;
|
||
const Lpx = getBicycleWheelbasePx();
|
||
const dPx = getBicycleWheelDiameterPx();
|
||
const wheelL = Math.max(18, dPx * 0.65);
|
||
const wheelW = Math.max(8, dPx * 0.22);
|
||
const deltaDeg = Number(b.steer.preview_deg) || 0;
|
||
const delta = (deltaDeg * Math.PI) / 180;
|
||
|
||
const rearAbs = robotToAbs(0, 0);
|
||
const frontAbs = robotToAbs(Lpx, 0);
|
||
|
||
function roundRectLocal(ctx2, x, y, w2, h2, r2) {
|
||
const rr = Math.min(r2, w2 / 2, h2 / 2);
|
||
ctx2.beginPath();
|
||
ctx2.moveTo(x + rr, y);
|
||
ctx2.arcTo(x + w2, y, x + w2, y + h2, rr);
|
||
ctx2.arcTo(x + w2, y + h2, x, y + h2, rr);
|
||
ctx2.arcTo(x, y + h2, x, y, rr);
|
||
ctx2.arcTo(x, y, x + w2, y, rr);
|
||
ctx2.closePath();
|
||
}
|
||
|
||
function drawBicycleWheel(center, steerRad, label) {
|
||
ctx.save();
|
||
ctx.translate(center.x, center.y);
|
||
ctx.rotate(yaw + (steerRad || 0));
|
||
ctx.fillStyle = "rgba(255,255,255,0.12)";
|
||
ctx.strokeStyle = "rgba(255,255,255,0.32)";
|
||
ctx.lineWidth = 2;
|
||
roundRectLocal(ctx, -wheelL / 2, -wheelW / 2, wheelL, wheelW, 6);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
ctx.fillStyle = "rgba(232,238,252,0.75)";
|
||
ctx.font = "10px ui-sans-serif, system-ui";
|
||
ctx.fillText(label, wheelL / 2 + 6, 3);
|
||
ctx.restore();
|
||
}
|
||
|
||
// Wheelbase L (chassis line)
|
||
ctx.strokeStyle = "rgba(100, 160, 255, 0.75)";
|
||
ctx.lineWidth = 3;
|
||
ctx.beginPath();
|
||
ctx.moveTo(rearAbs.x, rearAbs.y);
|
||
ctx.lineTo(frontAbs.x, frontAbs.y);
|
||
ctx.stroke();
|
||
|
||
// ICR preview: tan(δ) = L/R → R = L/tan(δ), ICR at (0, R) in robot frame (y left)
|
||
if (Math.abs(deltaDeg) > 0.5) {
|
||
const Rpx = Lpx / Math.tan(Math.abs(delta));
|
||
const icrY = deltaDeg > 0 ? Rpx : -Rpx;
|
||
const icrAbs = robotToAbs(0, icrY);
|
||
ctx.strokeStyle = "rgba(255, 100, 100, 0.45)";
|
||
ctx.lineWidth = 1.5;
|
||
ctx.setLineDash([6, 6]);
|
||
ctx.beginPath();
|
||
ctx.moveTo(rearAbs.x, rearAbs.y);
|
||
ctx.lineTo(icrAbs.x, icrAbs.y);
|
||
ctx.moveTo(frontAbs.x, frontAbs.y);
|
||
ctx.lineTo(icrAbs.x, icrAbs.y);
|
||
ctx.stroke();
|
||
ctx.setLineDash([]);
|
||
ctx.fillStyle = "rgba(255, 90, 90, 0.95)";
|
||
ctx.beginPath();
|
||
ctx.arc(icrAbs.x, icrAbs.y, 5, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
ctx.font = "11px ui-sans-serif, system-ui";
|
||
ctx.fillStyle = "rgba(255, 140, 140, 0.95)";
|
||
ctx.fillText("ICR", icrAbs.x + 8, icrAbs.y - 4);
|
||
ctx.fillStyle = "rgba(232,238,252,0.65)";
|
||
ctx.font = "10px ui-sans-serif, system-ui";
|
||
ctx.fillText(`δ=${deltaDeg.toFixed(0)}°`, frontAbs.x + 10, frontAbs.y - 10);
|
||
}
|
||
|
||
drawBicycleWheel(rearAbs, 0, "rear");
|
||
drawBicycleWheel(frontAbs, delta, "steer");
|
||
|
||
// Rear axle marker (reference point)
|
||
ctx.fillStyle = "rgba(100, 160, 255, 0.9)";
|
||
ctx.beginPath();
|
||
ctx.arc(rearAbs.x, rearAbs.y, 4, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
}
|
||
|
||
// draw lidars
|
||
const iconR = 14;
|
||
const lidarAxisLen = 32;
|
||
const lidarHeadLen = 10;
|
||
state.lidars.forEach((l, idx) => {
|
||
ensureDefaultPose(l.id, idx);
|
||
const p = getLidarPoseAbs(l.id);
|
||
const isSelected = state.selectedId === l.id;
|
||
const absX = p.absX;
|
||
const absY = p.absY;
|
||
// theta_deg is also ROS-style (CCW around +Z). Convert to canvas angle by negating.
|
||
const lidarTheta = (-(Number(p.theta_deg || 0) * Math.PI) / 180);
|
||
const yawCanvas = yaw + lidarTheta;
|
||
|
||
ctx.fillStyle = isSelected ? "rgba(76,201,240,0.65)" : "rgba(255,255,255,0.20)";
|
||
ctx.strokeStyle = isSelected ? "rgba(76,201,240,0.95)" : "rgba(255,255,255,0.30)";
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.arc(absX, absY, iconR, 0, Math.PI * 2);
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
|
||
// LiDAR axes icon (same style as robot center), rotated by (robot yaw + lidar theta)
|
||
const lux = Math.cos(yawCanvas);
|
||
const luy = Math.sin(yawCanvas);
|
||
const lvx = Math.cos(yawCanvas - Math.PI / 2);
|
||
const lvy = Math.sin(yawCanvas - Math.PI / 2);
|
||
const lxEnd = { x: absX + lux * lidarAxisLen, y: absY + luy * lidarAxisLen };
|
||
const lyEnd = { x: absX + lvx * lidarAxisLen, y: absY + lvy * lidarAxisLen };
|
||
|
||
// Temporarily override arrowhead size for lidar
|
||
const savedHeadLen = headLen;
|
||
// Reuse arrow() but with local head length by inlining a small arrowhead draw
|
||
function arrowSmall(fromX, fromY, toX, toY, color) {
|
||
ctx.strokeStyle = color;
|
||
ctx.fillStyle = color;
|
||
ctx.lineWidth = 2.5;
|
||
ctx.beginPath();
|
||
ctx.moveTo(fromX, fromY);
|
||
ctx.lineTo(toX, toY);
|
||
ctx.stroke();
|
||
|
||
const ang = Math.atan2(toY - fromY, toX - fromX);
|
||
ctx.beginPath();
|
||
ctx.moveTo(toX, toY);
|
||
ctx.lineTo(toX - lidarHeadLen * Math.cos(ang - Math.PI / 7), toY - lidarHeadLen * Math.sin(ang - Math.PI / 7));
|
||
ctx.lineTo(toX - lidarHeadLen * Math.cos(ang + Math.PI / 7), toY - lidarHeadLen * Math.sin(ang + Math.PI / 7));
|
||
ctx.closePath();
|
||
ctx.fill();
|
||
}
|
||
|
||
arrowSmall(absX, absY, lxEnd.x, lxEnd.y, "rgba(255, 80, 80, 0.95)"); // x (red)
|
||
arrowSmall(absX, absY, lyEnd.x, lyEnd.y, "rgba(110, 255, 140, 0.95)"); // y (green)
|
||
|
||
ctx.fillStyle = "rgba(232,238,252,0.92)";
|
||
ctx.font = "12px ui-sans-serif, system-ui";
|
||
ctx.fillText(l.name, absX + iconR + 8, absY + 4);
|
||
|
||
// Relative coordinate label (to robot center)
|
||
ctx.fillStyle = isSelected ? "rgba(76,201,240,0.95)" : "rgba(232,238,252,0.70)";
|
||
ctx.font = "11px ui-sans-serif, system-ui";
|
||
ctx.fillText(`(${Number(p.x || 0).toFixed(0)}, ${Number(p.y || 0).toFixed(0)}, θ=${Number(p.theta_deg || 0).toFixed(0)}°)`, absX + iconR + 8, absY + 18);
|
||
});
|
||
|
||
// IMU sensors (diamond icon, violet)
|
||
const imuR = 12;
|
||
state.imus.forEach((im, idx) => {
|
||
ensureDefaultImuPose(im.id, idx);
|
||
const p = getImuPoseAbs(im.id);
|
||
if (!p) return;
|
||
const isSelected = state.selectedImuId === im.id;
|
||
const absX = p.absX;
|
||
const absY = p.absY;
|
||
const yawImu = (-(Number(p.yaw_deg || 0) * Math.PI)) / 180;
|
||
const yawCanvasImu = yaw + yawImu;
|
||
|
||
ctx.save();
|
||
ctx.translate(absX, absY);
|
||
ctx.rotate(yawCanvasImu);
|
||
ctx.fillStyle = isSelected ? "rgba(192, 132, 252, 0.75)" : "rgba(168, 85, 247, 0.45)";
|
||
ctx.strokeStyle = isSelected ? "rgba(233, 213, 255, 0.95)" : "rgba(216, 180, 254, 0.85)";
|
||
ctx.lineWidth = 2;
|
||
ctx.beginPath();
|
||
ctx.moveTo(0, -imuR);
|
||
ctx.lineTo(imuR, 0);
|
||
ctx.lineTo(0, imuR);
|
||
ctx.lineTo(-imuR, 0);
|
||
ctx.closePath();
|
||
ctx.fill();
|
||
ctx.stroke();
|
||
ctx.restore();
|
||
|
||
ctx.fillStyle = "rgba(232,238,252,0.92)";
|
||
ctx.font = "12px ui-sans-serif, system-ui";
|
||
ctx.fillText(im.name, absX + imuR + 8, absY + 4);
|
||
ctx.fillStyle = isSelected ? "rgba(216, 180, 254, 0.95)" : "rgba(232,238,252,0.70)";
|
||
ctx.font = "11px ui-sans-serif, system-ui";
|
||
ctx.fillText(
|
||
`(${Number(p.x || 0).toFixed(0)}, ${Number(p.y || 0).toFixed(0)}, z=${Number(p.z || 0).toFixed(2)})`,
|
||
absX + imuR + 8,
|
||
absY + 18,
|
||
);
|
||
if (im.enabled === false) {
|
||
ctx.fillStyle = "rgba(248, 113, 113, 0.9)";
|
||
ctx.font = "10px ui-sans-serif, system-ui";
|
||
ctx.fillText("OFF", absX - 14, absY - imuR - 4);
|
||
}
|
||
});
|
||
|
||
ctx.restore();
|
||
}
|
||
|
||
function hitTestCanvasTarget(x, y) {
|
||
const iconR = 16 / Math.max(state.view.scale, 0.12);
|
||
const r2 = iconR * iconR;
|
||
for (let i = state.imus.length - 1; i >= 0; i--) {
|
||
const im = state.imus[i];
|
||
const p = getImuPoseAbs(im.id);
|
||
if (!p) continue;
|
||
const dx = x - p.absX;
|
||
const dy = y - p.absY;
|
||
if (dx * dx + dy * dy <= r2) return { kind: "imu", id: im.id };
|
||
}
|
||
for (let i = state.lidars.length - 1; i >= 0; i--) {
|
||
const l = state.lidars[i];
|
||
const p = getLidarPoseAbs(l.id);
|
||
if (!p) continue;
|
||
const dx = x - p.absX;
|
||
const dy = y - p.absY;
|
||
if (dx * dx + dy * dy <= r2) return { kind: "lidar", id: l.id };
|
||
}
|
||
return null;
|
||
}
|
||
|
||
canvas.addEventListener("wheel", (evt) => {
|
||
evt.preventDefault();
|
||
const s = canvasScreenPoint(evt);
|
||
const factor = evt.deltaY < 0 ? 1.12 : 1 / 1.12;
|
||
zoomAtScreen(s.x, s.y, factor);
|
||
renderCanvas();
|
||
}, { passive: false });
|
||
|
||
canvas.addEventListener("mousedown", (evt) => {
|
||
if (evt.button !== 0) return;
|
||
const p = canvasPoint(evt);
|
||
|
||
if (evt.shiftKey) {
|
||
if (state.editFootprint && isCustomFootprintShape()) {
|
||
const vIdx = hitTestFootprintVertex(p.x, p.y, 12);
|
||
if (vIdx !== null) {
|
||
state.selectedFootprintVertex = vIdx;
|
||
const pts = getFootprintAbsPoints();
|
||
state.draggingFootprint = { index: vIdx, dx: p.x - pts[vIdx].x, dy: p.y - pts[vIdx].y };
|
||
setStatus(`Đỉnh #${vIdx} — kéo di chuyển, Delete hoặc «Xóa đỉnh»`);
|
||
updateFootprintVertexUI();
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
}
|
||
const s = canvasScreenPoint(evt);
|
||
state.panning = {
|
||
startSx: s.x,
|
||
startSy: s.y,
|
||
startPanX: state.view.panX,
|
||
startPanY: state.view.panY,
|
||
moved: false,
|
||
};
|
||
state.pendingFootprintClick =
|
||
state.editFootprint && isCustomFootprintShape() ? { sx: s.x, sy: s.y } : null;
|
||
canvasWrap.classList.add("panning");
|
||
evt.preventDefault();
|
||
return;
|
||
}
|
||
|
||
if (state.editFootprint && isCustomFootprintShape()) {
|
||
const vIdx = hitTestFootprintVertex(p.x, p.y, 12);
|
||
if (vIdx !== null) {
|
||
state.selectedFootprintVertex = vIdx;
|
||
const pts = getFootprintAbsPoints();
|
||
state.draggingFootprint = { index: vIdx, dx: p.x - pts[vIdx].x, dy: p.y - pts[vIdx].y };
|
||
setStatus(`Đỉnh #${vIdx} — kéo di chuyển, Delete hoặc «Xóa đỉnh»`);
|
||
updateFootprintVertexUI();
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
addFootprintVertexFromCanvas(p.x, p.y);
|
||
persistLayoutDebounced();
|
||
return;
|
||
}
|
||
if (state.editFootprint && !isCustomFootprintShape()) {
|
||
setStatus("Chọn «Tùy chỉnh» hoặc chỉnh thông số + Áp dụng hình dạng");
|
||
return;
|
||
}
|
||
|
||
const target = hitTestCanvasTarget(p.x, p.y);
|
||
if (!target) return;
|
||
if (target.kind === "imu") {
|
||
selectImuOnCanvas(target.id);
|
||
const pose = getImuPoseAbs(target.id);
|
||
state.dragging = { kind: "imu", id: target.id, dx: p.x - pose.absX, dy: p.y - pose.absY };
|
||
updateImuItemPoseUI(target.id);
|
||
} else {
|
||
selectLidarOnCanvas(target.id);
|
||
const pose = getLidarPoseAbs(target.id);
|
||
state.dragging = { kind: "lidar", id: target.id, dx: p.x - pose.absX, dy: p.y - pose.absY };
|
||
updateLidarItemPoseUI(target.id);
|
||
}
|
||
renderCanvas();
|
||
});
|
||
|
||
window.addEventListener("mousemove", (evt) => {
|
||
if (state.panning) {
|
||
const s = canvasScreenPoint(evt);
|
||
const dx = s.x - state.panning.startSx;
|
||
const dy = s.y - state.panning.startSy;
|
||
if (Math.abs(dx) > 3 || Math.abs(dy) > 3) {
|
||
state.panning.moved = true;
|
||
state.pendingFootprintClick = null;
|
||
}
|
||
state.view.panX = state.panning.startPanX + dx;
|
||
state.view.panY = state.panning.startPanY + dy;
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
|
||
const p = canvasPoint(evt);
|
||
|
||
if (state.draggingFootprint && isCustomFootprintShape()) {
|
||
const idx = state.draggingFootprint.index;
|
||
const nx = p.x - state.draggingFootprint.dx;
|
||
const ny = p.y - state.draggingFootprint.dy;
|
||
const rel = absToRobot(nx, ny);
|
||
ensureFootprint();
|
||
state.layout.robot.footprint[idx] = { x: rel.x, y: rel.y };
|
||
state.layout.robot.footprint_shape = "custom";
|
||
if (footprintShapeEl) footprintShapeEl.value = "custom";
|
||
updateFootprintPresetPanelVisibility();
|
||
renderCanvas();
|
||
return;
|
||
}
|
||
|
||
if (!state.dragging) return;
|
||
const id = state.dragging.id;
|
||
const nx = p.x - state.dragging.dx;
|
||
const ny = p.y - state.dragging.dy;
|
||
const rel = absToRobot(nx, ny);
|
||
if (state.dragging.kind === "imu") {
|
||
ensureDefaultImuPose(id, 0);
|
||
state.layout.imuPoses[id].x = rel.x;
|
||
state.layout.imuPoses[id].y = rel.y;
|
||
if (state.selectedImuId === id) setSelectedRelText();
|
||
updateImuItemPoseUI(id);
|
||
} else {
|
||
ensureDefaultPose(id, 0);
|
||
state.layout.lidarPoses[id].x = rel.x;
|
||
state.layout.lidarPoses[id].y = rel.y;
|
||
if (state.selectedId === id) setSelectedRelText();
|
||
updateLidarItemPoseUI(id);
|
||
}
|
||
renderCanvas();
|
||
});
|
||
|
||
window.addEventListener("mouseup", (evt) => {
|
||
const draggedLidarId = state.dragging?.kind === "lidar" ? state.dragging.id : null;
|
||
const draggedImuId = state.dragging?.kind === "imu" ? state.dragging.id : null;
|
||
|
||
if (state.panning) {
|
||
if (
|
||
state.pendingFootprintClick &&
|
||
!state.panning.moved &&
|
||
state.editFootprint &&
|
||
isCustomFootprintShape()
|
||
) {
|
||
const s = canvasScreenPoint(evt);
|
||
const p = screenToWorld(s.x, s.y);
|
||
ensureFootprint();
|
||
const rel = absToRobot(p.x, p.y);
|
||
state.layout.robot.footprint.push({ x: rel.x, y: rel.y });
|
||
state.selectedFootprintVertex = state.layout.robot.footprint.length - 1;
|
||
setStatus("Đã thêm đỉnh footprint (Shift+click)");
|
||
renderCanvas();
|
||
}
|
||
state.panning = null;
|
||
state.pendingFootprintClick = null;
|
||
canvasWrap.classList.remove("panning");
|
||
return;
|
||
}
|
||
const hadFootprintDrag = state.draggingFootprint !== null;
|
||
state.dragging = null;
|
||
state.draggingFootprint = null;
|
||
|
||
if (draggedLidarId) {
|
||
updateLidarItemPoseUI(draggedLidarId);
|
||
persistLayoutDebounced();
|
||
} else if (draggedImuId) {
|
||
updateImuItemPoseUI(draggedImuId);
|
||
persistLayoutDebounced();
|
||
} else if (hadFootprintDrag) {
|
||
persistLayoutDebounced();
|
||
}
|
||
});
|
||
|
||
window.addEventListener("keydown", (evt) => {
|
||
if (!state.editFootprint || !isCustomFootprintShape()) return;
|
||
if (evt.key === "Escape") {
|
||
state.selectedFootprintVertex = null;
|
||
updateFootprintVertexUI();
|
||
renderCanvas();
|
||
setStatus("Đã bỏ chọn đỉnh");
|
||
return;
|
||
}
|
||
if (evt.key !== "Delete" && evt.key !== "Backspace") return;
|
||
if (state.selectedFootprintVertex === null) return;
|
||
evt.preventDefault();
|
||
if (removeSelectedFootprintVertex()) persistLayoutDebounced();
|
||
});
|
||
|
||
function clamp(v, a, b) {
|
||
return Math.max(a, Math.min(b, v));
|
||
}
|
||
|
||
function escapeHtml(s) {
|
||
return String(s)
|
||
.replaceAll("&", "&")
|
||
.replaceAll("<", "<")
|
||
.replaceAll(">", ">")
|
||
.replaceAll('"', """)
|
||
.replaceAll("'", "'");
|
||
}
|
||
|
||
let loadAllInFlight = null;
|
||
|
||
function reconcileLidarPoses() {
|
||
if (!state.layout.lidarPoses) state.layout.lidarPoses = {};
|
||
const valid = new Set(state.lidars.map((l) => l.id));
|
||
Object.keys(state.layout.lidarPoses).forEach((id) => {
|
||
if (!valid.has(id)) delete state.layout.lidarPoses[id];
|
||
});
|
||
}
|
||
|
||
async function loadAll() {
|
||
if (loadAllInFlight) return loadAllInFlight;
|
||
|
||
loadAllInFlight = (async () => {
|
||
const st = await api("/api/state");
|
||
if (overviewBackendEl) overviewBackendEl.textContent = "OK";
|
||
state.activeLayoutId = st.active_layout_id || null;
|
||
state.activeLayoutName = st.active_layout_name || "";
|
||
state.layoutCatalog = st.layouts || [];
|
||
state.lidars = st.lidars || [];
|
||
state.imus = st.imus || [];
|
||
state.layout = st.layout || state.layout;
|
||
clearLayoutDirty();
|
||
renderLayoutSelect();
|
||
state.layout.map = state.layout.map || { width: 800, height: 600 };
|
||
state.layout.robot = state.layout.robot || { x: 400, y: 300, yaw_deg: 0, model: "diff" };
|
||
if (!state.layout.robot.model) state.layout.robot.model = "diff";
|
||
ensureDiffSchema();
|
||
ensureFootprint();
|
||
// migrate old schema (lidarPositions: {id:{x,y}}) -> lidarPoses: {id:{x,y,theta_deg}}
|
||
if (!state.layout.lidarPoses) state.layout.lidarPoses = {};
|
||
if (state.layout.lidarPositions && typeof state.layout.lidarPositions === "object") {
|
||
Object.entries(state.layout.lidarPositions).forEach(([id, p]) => {
|
||
if (!state.layout.lidarPoses[id] && p && typeof p === "object") {
|
||
state.layout.lidarPoses[id] = { x: Number(p.x), y: Number(p.y), theta_deg: 0 };
|
||
}
|
||
});
|
||
delete state.layout.lidarPositions;
|
||
}
|
||
state.layout.lidarPoses = state.layout.lidarPoses || {};
|
||
// migrate from older versions where lidarPoses stored ABS canvas x/y
|
||
if (!state.layout.lidarPosesFrame) {
|
||
Object.entries(state.layout.lidarPoses).forEach(([id, p]) => {
|
||
if (!p || typeof p !== "object") return;
|
||
const rel = absToRobot(Number(p.x || 0), Number(p.y || 0));
|
||
p.x = rel.x;
|
||
p.y = rel.y;
|
||
if (p.theta_deg === undefined) p.theta_deg = 0;
|
||
});
|
||
state.layout.lidarPosesFrame = "robot";
|
||
}
|
||
if (state.layout.lidarPosesFrame !== "robot") state.layout.lidarPosesFrame = "robot";
|
||
|
||
reconcileLidarPoses();
|
||
|
||
if (!state.layout.imuPoses) state.layout.imuPoses = {};
|
||
if (!state.layout.imuPosesFrame) state.layout.imuPosesFrame = "robot";
|
||
reconcileImuPoses();
|
||
|
||
let addedDefaultPoses = false;
|
||
state.lidars.forEach((l, idx) => {
|
||
if (!state.layout.lidarPoses[l.id]) {
|
||
ensureDefaultPose(l.id, idx);
|
||
addedDefaultPoses = true;
|
||
}
|
||
});
|
||
let addedDefaultImuPoses = false;
|
||
state.imus.forEach((im, idx) => {
|
||
if (!state.layout.imuPoses[im.id]) {
|
||
ensureDefaultImuPose(im.id, idx);
|
||
addedDefaultImuPoses = true;
|
||
}
|
||
});
|
||
if (addedDefaultPoses || addedDefaultImuPoses) await persistLayoutNow();
|
||
|
||
syncDiffFormFromState();
|
||
syncFootprintUIFromState();
|
||
|
||
if (state.selectedId && !state.lidars.find((l) => l.id === state.selectedId)) {
|
||
state.selectedId = null;
|
||
}
|
||
if (state.selectedImuId && !state.imus.find((im) => im.id === state.selectedImuId)) {
|
||
state.selectedImuId = null;
|
||
}
|
||
if (!state.selectedId && !state.selectedImuId) {
|
||
selectedText.textContent = "none";
|
||
}
|
||
setSelectedRelText();
|
||
renderList();
|
||
renderImuList();
|
||
if (overviewActiveLayoutEl) {
|
||
const name = state.activeLayoutName || state.activeLayoutId || "—";
|
||
overviewActiveLayoutEl.textContent = name;
|
||
}
|
||
if (overviewActiveModelEl) {
|
||
overviewActiveModelEl.textContent = state.layout?.robot?.model || "diff";
|
||
}
|
||
if (overviewActiveSensorsEl) {
|
||
overviewActiveSensorsEl.textContent = `${state.lidars.length} LiDAR • ${state.imus.length} IMU`;
|
||
}
|
||
if (!state.viewInitialized) {
|
||
fitViewToWorld();
|
||
state.viewInitialized = true;
|
||
}
|
||
renderCanvas();
|
||
})();
|
||
|
||
try {
|
||
return await loadAllInFlight;
|
||
} finally {
|
||
loadAllInFlight = null;
|
||
}
|
||
}
|
||
|
||
el("refreshBtn").addEventListener("click", async () => {
|
||
try {
|
||
state.viewInitialized = false;
|
||
await loadAll();
|
||
setStatus("Đã tải lại");
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
}
|
||
});
|
||
|
||
let lidarFormBusy = false;
|
||
|
||
function normalizeLidarFields(name, ip, port) {
|
||
return {
|
||
name: String(name || "").trim(),
|
||
ip: String(ip || "").trim(),
|
||
port: Number(port),
|
||
};
|
||
}
|
||
|
||
function findDuplicateLidar(name, ip, port, excludeId = null) {
|
||
const p = normalizeLidarFields(name, ip, port);
|
||
if (!p.name || !p.ip || !Number.isFinite(p.port)) return null;
|
||
return (
|
||
state.lidars.find(
|
||
(l) =>
|
||
l.id !== excludeId &&
|
||
String(l.name || "").trim() === p.name &&
|
||
String(l.ip || "").trim() === p.ip &&
|
||
Number(l.port) === p.port,
|
||
) || null
|
||
);
|
||
}
|
||
|
||
function setLidarFormHint(msg) {
|
||
if (!lidarFormHintEl) return;
|
||
if (!msg) {
|
||
lidarFormHintEl.hidden = true;
|
||
lidarFormHintEl.textContent = "";
|
||
return;
|
||
}
|
||
lidarFormHintEl.hidden = false;
|
||
lidarFormHintEl.textContent = msg;
|
||
}
|
||
|
||
async function submitAddLidar() {
|
||
if (lidarFormBusy) return;
|
||
lidarFormBusy = true;
|
||
|
||
const addBtn = el("addLidarBtn");
|
||
if (addBtn) addBtn.disabled = true;
|
||
|
||
const payload = normalizeLidarFields(el("name").value, el("ip").value, el("port").value);
|
||
if (!payload.name || !payload.ip) {
|
||
setLidarFormHint("Nhập đủ tên và IP.");
|
||
setStatus("Nhập đủ tên và IP");
|
||
lidarFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
return;
|
||
}
|
||
if (payload.port < 1 || payload.port > 65535) {
|
||
setLidarFormHint("Port phải từ 1 đến 65535.");
|
||
setStatus("Port không hợp lệ");
|
||
lidarFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
return;
|
||
}
|
||
|
||
const dup = findDuplicateLidar(payload.name, payload.ip, payload.port);
|
||
if (dup) {
|
||
const msg =
|
||
`LiDAR trùng (tên, IP, port): "${dup.name}" ${dup.ip}:${dup.port} — không thêm bản ghi mới.`;
|
||
setLidarFormHint(msg);
|
||
setStatus(msg);
|
||
lidarFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
return;
|
||
}
|
||
|
||
setLidarFormHint("");
|
||
|
||
try {
|
||
const created = await api("/api/lidars", { method: "POST", body: JSON.stringify(payload) });
|
||
state.lidars.push(created);
|
||
ensureDefaultPose(created.id, state.lidars.length - 1);
|
||
reconcileLidarPoses();
|
||
await persistLayoutNow();
|
||
el("name").value = "";
|
||
el("ip").value = "";
|
||
renderList();
|
||
renderCanvas();
|
||
setStatus("Đã thêm LiDAR");
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
} finally {
|
||
lidarFormBusy = false;
|
||
if (addBtn) addBtn.disabled = false;
|
||
}
|
||
}
|
||
|
||
function initLidarForm() {
|
||
const form = el("lidarForm");
|
||
if (form.dataset.bound === "1") return;
|
||
form.dataset.bound = "1";
|
||
|
||
form.addEventListener("submit", (evt) => {
|
||
evt.preventDefault();
|
||
void submitAddLidar();
|
||
});
|
||
el("addLidarBtn").addEventListener("click", (evt) => {
|
||
evt.preventDefault();
|
||
void submitAddLidar();
|
||
});
|
||
|
||
["name", "ip", "port"].forEach((id) => {
|
||
el(id).addEventListener("input", () => setLidarFormHint(""));
|
||
});
|
||
}
|
||
|
||
robotModelEl.addEventListener("change", () => {
|
||
const m = robotModelEl.value || "diff";
|
||
state.layout.robot.model = m;
|
||
if (m === "bicycle") applyBicycleFormToState();
|
||
else applyDiffFormToState();
|
||
syncDiffFormFromState();
|
||
markLayoutDirty();
|
||
renderCanvas();
|
||
});
|
||
|
||
[
|
||
wheelSeparationMEl,
|
||
wheelRadiusMEl,
|
||
scaleMPerPxEl,
|
||
wheelSeparationMultEl,
|
||
wheelRadiusMultEl,
|
||
cmdVelTimeoutEl,
|
||
linearMaxVelEl,
|
||
linearMinVelEl,
|
||
linearMaxAccelEl,
|
||
angularMaxVelEl,
|
||
angularMaxAccelEl,
|
||
].forEach((node) => {
|
||
node.addEventListener("change", onDiffFieldChange);
|
||
node.addEventListener("input", () => {
|
||
applyDiffFormToState();
|
||
markLayoutDirty();
|
||
renderCanvas();
|
||
});
|
||
});
|
||
|
||
[
|
||
bicycleWheelbaseMEl,
|
||
bicycleWheelRadiusMEl,
|
||
bicycleScaleMPerPxEl,
|
||
bicycleSteerPreviewDegEl,
|
||
bicycleSteerMaxDegEl,
|
||
bicycleCmdVelTimeoutEl,
|
||
bicycleLinearMaxVelEl,
|
||
bicycleLinearMaxAccelEl,
|
||
].forEach((node) => {
|
||
if (!node) return;
|
||
node.addEventListener("change", onBicycleFieldChange);
|
||
node.addEventListener("input", () => {
|
||
applyBicycleFormToState();
|
||
markLayoutDirty();
|
||
renderCanvas();
|
||
});
|
||
});
|
||
|
||
editFootprintBtn.addEventListener("click", () => {
|
||
setEditFootprintMode(!state.editFootprint);
|
||
setStatus(state.editFootprint ? "Chế độ sửa footprint: bật" : "Chế độ sửa footprint: tắt");
|
||
});
|
||
|
||
function initLidarListPanelCollapse() {
|
||
try {
|
||
const saved = localStorage.getItem("lidarListPanelCollapsed");
|
||
if (saved === "1") setLidarListPanelCollapsed(true);
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
const toggle = () => setLidarListPanelCollapsed(!state.lidarListPanelCollapsed);
|
||
lidarListCardToggle.addEventListener("click", toggle);
|
||
lidarListCardToggle.addEventListener("keydown", (evt) => {
|
||
if (evt.key === "Enter" || evt.key === " ") {
|
||
evt.preventDefault();
|
||
toggle();
|
||
}
|
||
});
|
||
}
|
||
|
||
function initRobotModelPanelCollapse() {
|
||
try {
|
||
const saved = localStorage.getItem("robotModelPanelCollapsed");
|
||
if (saved === "1") setRobotModelPanelCollapsed(true);
|
||
} catch {
|
||
/* ignore */
|
||
}
|
||
const toggle = () => setRobotModelPanelCollapsed(!state.robotModelPanelCollapsed);
|
||
robotModelCardToggle.addEventListener("click", toggle);
|
||
robotModelCardToggle.addEventListener("keydown", (evt) => {
|
||
if (evt.key === "Enter" || evt.key === " ") {
|
||
evt.preventDefault();
|
||
toggle();
|
||
}
|
||
});
|
||
}
|
||
|
||
initLayoutManagerEvents();
|
||
initNavigation();
|
||
initSplitPane();
|
||
initLidarForm();
|
||
initMotorWheelsEvents();
|
||
initBicycleMotorWheelsEvents();
|
||
initFootprintEvents();
|
||
initLidarListEvents();
|
||
initImuListEvents();
|
||
initImuForm();
|
||
initLidarListPanelCollapse();
|
||
initImuListPanelCollapse();
|
||
initRobotModelPanelCollapse();
|
||
|
||
if (typeof ResizeObserver !== "undefined") {
|
||
let resizeRaf = 0;
|
||
new ResizeObserver(() => {
|
||
if (resizeRaf) cancelAnimationFrame(resizeRaf);
|
||
resizeRaf = requestAnimationFrame(() => {
|
||
resizeRaf = 0;
|
||
renderCanvas();
|
||
});
|
||
}).observe(canvasWrap);
|
||
} else {
|
||
window.addEventListener("resize", () => renderCanvas());
|
||
}
|
||
|
||
window.addEventListener("keydown", (evt) => {
|
||
if (evt.key === "Shift") canvasWrap.classList.add("shift-pan");
|
||
});
|
||
window.addEventListener("keyup", (evt) => {
|
||
if (evt.key === "Shift") canvasWrap.classList.remove("shift-pan");
|
||
});
|
||
|
||
saveLayoutBtn.addEventListener("click", async () => {
|
||
try {
|
||
await saveCurrentLayout();
|
||
setStatus(`Đã lưu layout «${state.activeLayoutName || ""}»`);
|
||
} catch (e) {
|
||
setStatus(`Lỗi: ${e.message}`);
|
||
}
|
||
});
|
||
|
||
(async () => {
|
||
try {
|
||
await api("/api/health");
|
||
await loadMotorCatalog();
|
||
await loadAll();
|
||
selectedText.textContent = "none";
|
||
selectedRelText.textContent = "—";
|
||
setStatus("Sẵn sàng");
|
||
} catch (e) {
|
||
const msg = String(e.message || e);
|
||
if (overviewBackendEl) overviewBackendEl.textContent = `Lỗi: ${msg}`;
|
||
if (msg.includes("stack") || msg.includes("Maximum call")) {
|
||
setStatus(`Lỗi JavaScript: ${msg}`);
|
||
} else {
|
||
setStatus(`Không kết nối được backend: ${msg}`);
|
||
}
|
||
}
|
||
})();
|
||
|