HiepLM/App
1
0
Fork 0
Code Issues Pull Requests Actions 1 Packages Projects Releases Wiki Activity

Add IMU layout

Browse Source
This commit is contained in:
HiepLM
2026-05-29 17:12:22 +07:00
parent 194fffcdf7
commit 4f8d8148f7
7 changed files with 827 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
src/main.cpp
View File

@@ -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<json> 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<size_t> 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<size_t> find_lidar_index(const json& state, const std::stri
return std::nullopt;
}
static std::optional<size_t> 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<std::string>() == 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<std::string>().empty())
{
err = "name is required";
return false;
}
if (!payload.contains("frame_id") || !payload["frame_id"].is_string() ||
payload["frame_id"].get<std::string>().empty())
{
err = "frame_id is required";
return false;
}
if (!payload.contains("topic") || !payload["topic"].is_string() || payload["topic"].get<std::string>().empty())
{
err = "topic is required";
return false;
}
if (payload.contains("source") && payload["source"].is_string())
{
const std::string src = payload["source"].get<std::string>();
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<std::string>() == *exclude_id)
continue;
if (!im.contains("frame_id"))
continue;
if (trim_copy(im["frame_id"].get<std::string>()) == 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<bool>();
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<std::string>());
const std::string frame_id = trim_copy(payload["frame_id"].get<std::string>());
const std::string topic = trim_copy(payload["topic"].get<std::string>());
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<std::string>()
: "external";
const bool enabled = !payload.contains("enabled") || payload["enabled"].get<bool>();
const double rate_hz =
payload.contains("rate_hz") && payload["rate_hz"].is_number() ? payload["rate_hz"].get<double>() : 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<std::string>());
const std::string frame_id = trim_copy(payload["frame_id"].get<std::string>());
const std::string topic = trim_copy(payload["topic"].get<std::string>());
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<json::difference_type>(*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