diff --git a/README.md b/README.md index e844db2..fd72d76 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,9 @@ Chức năng: - Đăng ký danh sách cảm biến LiDAR (tên, ip, port) -- Kéo thả icon LiDAR trên canvas để set vị trí tương đối (theo px) -- Lưu cấu hình xuống file JSON +- Đăng ký IMU (tên, frame_id, topic, nguồn) và pose trên robot +- Kéo thả icon LiDAR/IMU trên canvas để set vị trí (robot frame) +- Nhiều layout — mỗi layout lưu tại `data/models/{id}.json`; catalog trong `data/state.json` ## Build diff --git a/data/models/a07ab938d9029ef1.json b/data/models/a07ab938d9029ef1.json index de1edf0..c6abbf7 100644 --- a/data/models/a07ab938d9029ef1.json +++ b/data/models/a07ab938d9029ef1.json @@ -1,7 +1,27 @@ { "created_at": "2026-05-29T08:27:25Z", "id": "a07ab938d9029ef1", + "imus": [ + { + "enabled": true, + "frame_id": "imu_link", + "id": "f7ddb6d2c3c1c5cf", + "name": "IMU test", + "rate_hz": 100, + "source": "onboard", + "topic": "imu/data" + } + ], "layout": { + "imuPoses": { + "f7ddb6d2c3c1c5cf": { + "x": 196.14886948882076, + "y": 0.1286840744156286, + "yaw_deg": 0, + "z": 0.1 + } + }, + "imuPosesFrame": "robot", "lidarPoses": { "02c4b7f4de7bd639": { "theta_deg": 45, @@ -200,5 +220,5 @@ } ], "name": "Mặc định", - "updated_at": "2026-05-29T08:39:03Z" + "updated_at": "2026-05-29T10:09:07Z" } diff --git a/data/models/ea89e39c835c0557.json b/data/models/ea89e39c835c0557.json index 8e0fe63..62be4af 100644 --- a/data/models/ea89e39c835c0557.json +++ b/data/models/ea89e39c835c0557.json @@ -1,7 +1,27 @@ { "created_at": "2026-05-29T08:40:51Z", "id": "ea89e39c835c0557", + "imus": [ + { + "enabled": true, + "frame_id": "imu_link", + "id": "719a21772e114466", + "name": "IMU", + "rate_hz": 100, + "source": "external", + "topic": "imu/data" + } + ], "layout": { + "imuPoses": { + "719a21772e114466": { + "x": 0.06910131801805619, + "y": 0.8135664703630141, + "yaw_deg": 0, + "z": 0.1 + } + }, + "imuPosesFrame": "robot", "lidarPoses": { "40235913b52d8101": { "theta_deg": -135, @@ -173,5 +193,5 @@ } ], "name": "T800", - "updated_at": "2026-05-29T08:44:03Z" + "updated_at": "2026-05-29T10:11:49Z" } diff --git a/data/state.json b/data/state.json index df4d6b0..97ef6d3 100644 --- a/data/state.json +++ b/data/state.json @@ -1,21 +1,23 @@ { - "active_layout_id": "a07ab938d9029ef1", + "active_layout_id": "ea89e39c835c0557", "layouts": [ { "created_at": "2026-05-29T08:27:25Z", "id": "a07ab938d9029ef1", + "imu_count": 1, "lidar_count": 3, "model": "bicycle", "name": "Mặc định", - "updated_at": "2026-05-29T08:39:03Z" + "updated_at": "2026-05-29T10:09:07Z" }, { "created_at": "2026-05-29T08:40:51Z", "id": "ea89e39c835c0557", + "imu_count": 1, "lidar_count": 2, "model": "diff", "name": "T800", - "updated_at": "2026-05-29T08:44:03Z" + "updated_at": "2026-05-29T10:11:49Z" } ], "version": 3 diff --git a/src/main.cpp b/src/main.cpp index e37b2a8..ab87896 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -197,10 +197,15 @@ static json default_layout_object() {"map", {{"width", 800}, {"height", 600}}}, {"lidarPositions", json::object()}, {"lidarPoses", json::object()}, - {"lidarPosesFrame", "robot"}}; + {"lidarPosesFrame", "robot"}, + {"imuPoses", json::object()}, + {"imuPosesFrame", "robot"}}; } -static json make_layout_profile(const std::string& name, const json& layout, const json& lidars) +static json make_layout_profile(const std::string& name, + const json& layout, + const json& lidars, + const json& imus = json::array()) { const std::string ts = now_iso8601(); return json{{"id", new_id()}, @@ -208,7 +213,8 @@ static json make_layout_profile(const std::string& name, const json& layout, con {"created_at", ts}, {"updated_at", ts}, {"layout", layout}, - {"lidars", lidars}}; + {"lidars", lidars}, + {"imus", imus}}; } static std::string trim_copy(const std::string& s) @@ -271,12 +277,15 @@ static json catalog_entry_from_profile(const json& profile) const json& layout = profile.contains("layout") ? profile["layout"] : json::object(); const size_t lidar_count = profile.contains("lidars") && profile["lidars"].is_array() ? profile["lidars"].size() : 0; + const size_t imu_count = + profile.contains("imus") && profile["imus"].is_array() ? profile["imus"].size() : 0; return json{{"id", profile["id"]}, {"name", profile["name"]}, {"model", profile_model_from_layout(layout)}, {"created_at", profile.value("created_at", "")}, {"updated_at", profile.value("updated_at", "")}, - {"lidar_count", lidar_count}}; + {"lidar_count", lidar_count}, + {"imu_count", imu_count}}; } static std::optional load_profile_from_disk(const AppState& app, const std::string& id) @@ -337,10 +346,13 @@ static void load_active_cache(AppState& app) profile["layout"] = default_layout_object(); if (!profile.contains("lidars") || !profile["lidars"].is_array()) profile["lidars"] = json::array(); + if (!profile.contains("imus") || !profile["imus"].is_array()) + profile["imus"] = json::array(); ensure_layout_schema(profile["layout"]); state["layout"] = profile["layout"]; state["lidars"] = profile["lidars"]; + state["imus"] = profile["imus"]; } static bool persist_active_profile(AppState& app) @@ -358,6 +370,7 @@ static bool persist_active_profile(AppState& app) profile["updated_at"] = now_iso8601(); profile["layout"] = state["layout"]; profile["lidars"] = state["lidars"]; + profile["imus"] = state.contains("imus") && state["imus"].is_array() ? state["imus"] : json::array(); ensure_layout_schema(profile["layout"]); if (!save_profile_to_disk(app, profile)) return false; @@ -530,6 +543,7 @@ static void bootstrap_default_state(AppState& app) {"layouts", json::array({catalog_entry_from_profile(profile)})}}; app.state["layout"] = profile["layout"]; app.state["lidars"] = profile["lidars"]; + app.state["imus"] = profile.contains("imus") ? profile["imus"] : json::array(); } static void ensure_layout_schema(json& layout) @@ -546,6 +560,10 @@ static void ensure_layout_schema(json& layout) layout["lidarPoses"] = json::object(); if (!layout.contains("lidarPosesFrame")) layout["lidarPosesFrame"] = "robot"; + if (!layout.contains("imuPoses") || !layout["imuPoses"].is_object()) + layout["imuPoses"] = json::object(); + if (!layout.contains("imuPosesFrame")) + layout["imuPosesFrame"] = "robot"; auto& robot = layout["robot"]; if (!robot.contains("x")) @@ -803,7 +821,9 @@ static bool save_app_state(AppState& app) static std::optional find_lidar_index(const json& state, const std::string& id) { - const auto& lidars = state.at("lidars"); + if (!state.contains("lidars") || !state["lidars"].is_array()) + return std::nullopt; + const auto& lidars = state["lidars"]; for (size_t i = 0; i < lidars.size(); i++) { const auto& l = lidars[i]; @@ -813,6 +833,20 @@ static std::optional find_lidar_index(const json& state, const std::stri return std::nullopt; } +static std::optional find_imu_index(const json& state, const std::string& id) +{ + if (!state.contains("imus") || !state["imus"].is_array()) + return std::nullopt; + const auto& imus = state["imus"]; + for (size_t i = 0; i < imus.size(); i++) + { + const auto& im = imus[i]; + if (im.is_object() && im.contains("id") && im["id"].is_string() && im["id"].get() == id) + return i; + } + return std::nullopt; +} + static void json_error(httplib::Response& res, int status, const std::string& msg) { res.status = status; @@ -884,6 +918,67 @@ static bool lidar_triplet_exists(const json& state, return false; } +static bool validate_imu_payload(const json& payload, std::string& err) +{ + if (!payload.is_object()) + { + err = "payload must be a JSON object"; + return false; + } + if (!payload.contains("name") || !payload["name"].is_string() || payload["name"].get().empty()) + { + err = "name is required"; + return false; + } + if (!payload.contains("frame_id") || !payload["frame_id"].is_string() || + payload["frame_id"].get().empty()) + { + err = "frame_id is required"; + return false; + } + if (!payload.contains("topic") || !payload["topic"].is_string() || payload["topic"].get().empty()) + { + err = "topic is required"; + return false; + } + if (payload.contains("source") && payload["source"].is_string()) + { + const std::string src = payload["source"].get(); + if (src != "external" && src != "lidar_builtin" && src != "onboard") + { + err = "source must be external, lidar_builtin, or onboard"; + return false; + } + } + if (payload.contains("rate_hz") && !payload["rate_hz"].is_number()) + { + err = "rate_hz must be a number"; + return false; + } + return true; +} + +static bool imu_frame_exists(const json& state, + const std::string& frame_id, + const std::string* exclude_id = nullptr) +{ + if (!state.contains("imus") || !state["imus"].is_array()) + return false; + const std::string f = trim_copy(frame_id); + for (const auto& im : state["imus"]) + { + if (!im.is_object()) + continue; + if (exclude_id && im.contains("id") && im["id"].get() == *exclude_id) + continue; + if (!im.contains("frame_id")) + continue; + if (trim_copy(im["frame_id"].get()) == f) + return true; + } + return false; +} + static void mount_static(httplib::Server& svr, const fs::path& www_root) { svr.Get(R"(/(.*))", [www_root](const httplib::Request& req, httplib::Response& res) { @@ -959,7 +1054,8 @@ int main(int argc, char** argv) {"active_layout_name", active_name}, {"layouts", build_layouts_catalog(app.state)}, {"layout", app.state["layout"]}, - {"lidars", app.state["lidars"]}}; + {"lidars", app.state["lidars"]}, + {"imus", app.state.contains("imus") ? app.state["imus"] : json::array()}}; res.set_header("Content-Type", "application/json; charset=utf-8"); res.body = response.dump(); }); @@ -996,12 +1092,14 @@ int main(int argc, char** argv) const bool clone = payload.contains("clone") && payload["clone"].is_boolean() && payload["clone"].get(); json layout = default_layout_object(); json lidars = json::array(); + json imus = json::array(); if (clone) { layout = app.state["layout"]; lidars = app.state["lidars"]; + imus = app.state.contains("imus") && app.state["imus"].is_array() ? app.state["imus"] : json::array(); } - json profile = make_layout_profile(name, layout, lidars); + json profile = make_layout_profile(name, layout, lidars, imus); ensure_layout_schema(profile["layout"]); if (!save_profile_to_disk(app, profile)) return json_error(res, 500, "failed to save layout file"); @@ -1217,6 +1315,10 @@ int main(int argc, char** argv) profile["layout"] = payload["layout"]; if (payload.contains("lidars") && payload["lidars"].is_array()) profile["lidars"] = payload["lidars"]; + if (payload.contains("imus") && payload["imus"].is_array()) + profile["imus"] = payload["imus"]; + if (!profile.contains("imus") || !profile["imus"].is_array()) + profile["imus"] = json::array(); ensure_layout_schema(profile["layout"]); touch_profile(profile); if (!save_profile_to_disk(app, profile)) @@ -1229,6 +1331,7 @@ int main(int argc, char** argv) { app.state["layout"] = profile["layout"]; app.state["lidars"] = profile["lidars"]; + app.state["imus"] = profile["imus"]; } save_state(app); @@ -1236,6 +1339,122 @@ int main(int argc, char** argv) res.body = profile.dump(); }); + svr.Get("/api/imus", [&app](const httplib::Request&, httplib::Response& res) { + add_cors(res); + ensure_schema(app); + res.set_header("Content-Type", "application/json; charset=utf-8"); + res.body = (app.state.contains("imus") ? app.state["imus"] : json::array()).dump(); + }); + + svr.Post("/api/imus", [&app](const httplib::Request& req, httplib::Response& res) { + add_cors(res); + json payload; + try + { + payload = json::parse(req.body); + } + catch (...) + { + return json_error(res, 400, "invalid JSON"); + } + std::string err; + if (!validate_imu_payload(payload, err)) + return json_error(res, 400, err); + + const std::string name = trim_copy(payload["name"].get()); + const std::string frame_id = trim_copy(payload["frame_id"].get()); + const std::string topic = trim_copy(payload["topic"].get()); + if (imu_frame_exists(app.state, frame_id)) + return json_error(res, 409, "imu with same frame_id already exists"); + + if (!app.state.contains("imus") || !app.state["imus"].is_array()) + app.state["imus"] = json::array(); + + const std::string source = + payload.contains("source") && payload["source"].is_string() ? payload["source"].get() + : "external"; + const bool enabled = !payload.contains("enabled") || payload["enabled"].get(); + const double rate_hz = + payload.contains("rate_hz") && payload["rate_hz"].is_number() ? payload["rate_hz"].get() : 100.0; + + json imu = {{"id", new_id()}, + {"name", name}, + {"frame_id", frame_id}, + {"topic", topic}, + {"source", source}, + {"enabled", enabled}, + {"rate_hz", rate_hz}}; + app.state["imus"].push_back(imu); + if (!save_app_state(app)) + return json_error(res, 500, "failed to save layout"); + + res.status = 201; + res.set_header("Content-Type", "application/json; charset=utf-8"); + res.body = imu.dump(); + }); + + svr.Put(R"(/api/imus/([0-9a-fA-F]+))", [&app](const httplib::Request& req, httplib::Response& res) { + add_cors(res); + const std::string id = req.matches[1].str(); + + json payload; + try + { + payload = json::parse(req.body); + } + catch (...) + { + return json_error(res, 400, "invalid JSON"); + } + std::string err; + if (!validate_imu_payload(payload, err)) + return json_error(res, 400, err); + + auto idx = find_imu_index(app.state, id); + if (!idx) + return json_error(res, 404, "imu not found"); + + const std::string name = trim_copy(payload["name"].get()); + const std::string frame_id = trim_copy(payload["frame_id"].get()); + const std::string topic = trim_copy(payload["topic"].get()); + if (imu_frame_exists(app.state, frame_id, &id)) + return json_error(res, 409, "imu with same frame_id already exists"); + + auto& imu = app.state["imus"][*idx]; + imu["name"] = name; + imu["frame_id"] = frame_id; + imu["topic"] = topic; + if (payload.contains("source") && payload["source"].is_string()) + imu["source"] = payload["source"]; + if (payload.contains("enabled") && payload["enabled"].is_boolean()) + imu["enabled"] = payload["enabled"]; + if (payload.contains("rate_hz") && payload["rate_hz"].is_number()) + imu["rate_hz"] = payload["rate_hz"]; + if (!save_app_state(app)) + return json_error(res, 500, "failed to save layout"); + + res.set_header("Content-Type", "application/json; charset=utf-8"); + res.body = imu.dump(); + }); + + svr.Delete(R"(/api/imus/([0-9a-fA-F]+))", [&app](const httplib::Request& req, httplib::Response& res) { + add_cors(res); + const std::string id = req.matches[1].str(); + auto idx = find_imu_index(app.state, id); + if (!idx) + return json_error(res, 404, "imu not found"); + + app.state["imus"].erase(app.state["imus"].begin() + static_cast(*idx)); + if (app.state.contains("layout") && app.state["layout"].is_object()) + { + if (app.state["layout"].contains("imuPoses") && app.state["layout"]["imuPoses"].is_object()) + app.state["layout"]["imuPoses"].erase(id); + } + if (!save_app_state(app)) + return json_error(res, 500, "failed to save layout"); + res.status = 204; + }); + mount_static(svr, www_root); // Console hint diff --git a/www/app.js b/www/app.js index 0afc8ba..21a61e5 100644 --- a/www/app.js +++ b/www/app.js @@ -59,6 +59,10 @@ 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"); @@ -71,6 +75,7 @@ const selectedRelText = el("selectedRelText"); const state = { lidars: [], + imus: [], layout: { map: { width: 800, height: 600 }, robot: { x: 400, y: 300, yaw_deg: 0 }, @@ -78,10 +83,13 @@ const state = { // 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, // {id, dx, dy} lidar + dragging: null, // {kind:'lidar'|'imu', id, dx, dy} draggingFootprint: null, // {index, dx, dy} selectedId: null, + selectedImuId: null, selectedFootprintVertex: null, editFootprint: false, lidarListPanelCollapsed: false, @@ -91,6 +99,8 @@ const state = { 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 } @@ -436,6 +446,72 @@ function ensureDefaultPose(id, idx) { }; } +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; @@ -1499,7 +1575,7 @@ async function persistLayoutNow() { if (state.activeLayoutId) { await api(`/api/layouts/${state.activeLayoutId}`, { method: "PUT", - body: JSON.stringify({ layout: state.layout, lidars: state.lidars }), + body: JSON.stringify({ layout: state.layout, lidars: state.lidars, imus: state.imus }), }); } else { await api("/api/layout", { method: "PUT", body: JSON.stringify(state.layout) }); @@ -1667,10 +1743,7 @@ function initLidarListEvents() { return; } if (action === "select") { - state.selectedId = id; - selectedText.textContent = id; - setSelectedRelText(); - refreshLidarSelectionUI(); + selectLidarOnCanvas(id); renderCanvas(); return; } @@ -1681,10 +1754,7 @@ function initLidarListEvents() { 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) { - state.selectedId = null; - selectedText.textContent = "none"; - } + if (state.selectedId === id) clearCanvasSelection(); setSelectedRelText(); renderList(); renderCanvas(); @@ -1705,6 +1775,184 @@ function initLidarListEvents() { }); } +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); @@ -1723,6 +1971,17 @@ function setEditFootprintMode(on) { } 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; @@ -1801,6 +2060,121 @@ function renderList() { .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 ? `selected` : ""; + 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 = `
Chưa có IMU
Thêm IMU ở form phía trên.
`; + 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 ? `selected` : ""; + 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 ` +
+
+
+ +
+
${escapeHtml(im.name)} ${selected}
+
${escapeHtml(im.frame_id)} • ${escapeHtml(im.topic)} • ${srcLabel}${enabledTxt} • ${posTxt}
+
+
+
+ + +
+
+
+
+
+ X + +
+
+ Y + +
+
+ Z + +
+
+ ψ + +
+
+
+
`; + }) + .join(""); +} + let lastCanvasW = 0; let lastCanvasH = 0; @@ -2251,19 +2625,72 @@ function renderCanvas() { 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 hitTest(x, y) { +function hitTestCanvasTarget(x, y) { const iconR = 16 / Math.max(state.view.scale, 0.12); - // search top-most: reverse order + 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 <= iconR * iconR) return l.id; + if (dx * dx + dy * dy <= r2) return { kind: "lidar", id: l.id }; } return null; } @@ -2328,15 +2755,19 @@ canvas.addEventListener("mousedown", (evt) => { return; } - const id = hitTest(p.x, p.y); - if (!id) return; - state.selectedId = id; - selectedText.textContent = id; - setSelectedRelText(); - refreshLidarSelectionUI(); - const pose = getLidarPoseAbs(id); - state.dragging = { id, dx: p.x - pose.absX, dy: p.y - pose.absY }; - updateLidarItemPoseUI(id); + 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(); }); @@ -2375,17 +2806,26 @@ window.addEventListener("mousemove", (evt) => { const id = state.dragging.id; const nx = p.x - state.dragging.dx; const ny = p.y - state.dragging.dy; - ensureDefaultPose(id, 0); const rel = absToRobot(nx, ny); - state.layout.lidarPoses[id].x = rel.x; - state.layout.lidarPoses[id].y = rel.y; - if (state.selectedId === id) setSelectedRelText(); - updateLidarItemPoseUI(id); + 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?.id ?? null; + const draggedLidarId = state.dragging?.kind === "lidar" ? state.dragging.id : null; + const draggedImuId = state.dragging?.kind === "imu" ? state.dragging.id : null; if (state.panning) { if ( @@ -2415,6 +2855,9 @@ window.addEventListener("mouseup", (evt) => { if (draggedLidarId) { updateLidarItemPoseUI(draggedLidarId); persistLayoutDebounced(); + } else if (draggedImuId) { + updateImuItemPoseUI(draggedImuId); + persistLayoutDebounced(); } else if (hadFootprintDrag) { persistLayoutDebounced(); } @@ -2467,6 +2910,7 @@ async function loadAll() { 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(); @@ -2501,6 +2945,10 @@ async function loadAll() { 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]) { @@ -2508,17 +2956,30 @@ async function loadAll() { addedDefaultPoses = true; } }); - if (addedDefaultPoses) await persistLayoutNow(); + 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 (!state.viewInitialized) { fitViewToWorld(); state.viewInitialized = true; @@ -2747,7 +3208,10 @@ initMotorWheelsEvents(); initBicycleMotorWheelsEvents(); initFootprintEvents(); initLidarListEvents(); +initImuListEvents(); +initImuForm(); initLidarListPanelCollapse(); +initImuListPanelCollapse(); initRobotModelPanelCollapse(); if (typeof ResizeObserver !== "undefined") { diff --git a/www/index.html b/www/index.html index 017054c..5492223 100644 --- a/www/index.html +++ b/www/index.html @@ -121,6 +121,64 @@ +
+
+
+
IMU
+
Cảm biến quán tính — frame, topic và pose trên robot.
+
+ +
+ +
+
+
+ + +
+
+ + +
+
+ + +
+
+ + +
+
+ + +
+
+ +
+
+ +
+ +
+ +
+
+
+