save mission

This commit is contained in:
2026-06-13 12:06:48 +07:00
parent 10f4c36c23
commit 7c505e919c
4 changed files with 1190 additions and 0 deletions

View File

@@ -0,0 +1,723 @@
#include "mission/mission_service.hpp"
#include "util/file_util.hpp"
#include "util/id_util.hpp"
#include "util/string_util.hpp"
#include <algorithm>
#include <chrono>
#include <thread>
namespace lm {
namespace {
constexpr int kLogMax = 200;
nlohmann::json defaultDoc()
{
return nlohmann::json{{"version", 1},
{"missions", nlohmann::json::array()},
{"groups", nlohmann::json::array({"Missions", "Move", "Logic", "I/O", "Cart", "Misc"})},
{"triggers", nlohmann::json::array()},
{"dashboard", nlohmann::json{{"widgets", nlohmann::json::array()}}}};
}
} // namespace
MissionService::MissionService(std::filesystem::path data_dir)
: data_dir_(std::move(data_dir)), missions_path_(data_dir_ / "missions.json")
{
load();
worker_ = std::thread([this] { workerLoop(); });
}
MissionService::~MissionService()
{
stop_thread_ = true;
cv_.notify_all();
if (worker_.joinable())
worker_.join();
}
void MissionService::load()
{
std::lock_guard lock(mu_);
const auto raw = FileUtil::readBinary(missions_path_);
if (raw.empty())
{
doc_ = defaultDoc();
return;
}
try
{
doc_ = nlohmann::json::parse(raw);
}
catch (...)
{
doc_ = defaultDoc();
}
ensureDocSchema();
queue_ = nlohmann::json::array();
action_log_ = nlohmann::json::array();
}
void MissionService::ensureDocSchema()
{
if (!doc_.is_object())
doc_ = defaultDoc();
if (!doc_.contains("version"))
doc_["version"] = 1;
if (!doc_.contains("missions") || !doc_["missions"].is_array())
doc_["missions"] = nlohmann::json::array();
if (!doc_.contains("groups") || !doc_["groups"].is_array())
doc_["groups"] = defaultDoc()["groups"];
if (!doc_.contains("triggers") || !doc_["triggers"].is_array())
doc_["triggers"] = nlohmann::json::array();
if (!doc_.contains("dashboard") || !doc_["dashboard"].is_object())
doc_["dashboard"] = nlohmann::json{{"widgets", nlohmann::json::array()}};
if (!doc_["dashboard"].contains("widgets") || !doc_["dashboard"]["widgets"].is_array())
doc_["dashboard"]["widgets"] = nlohmann::json::array();
}
bool MissionService::saveDocument() const
{
std::lock_guard lock(mu_);
std::error_code ec;
std::filesystem::create_directories(data_dir_, ec);
auto body = doc_.dump(2);
body.push_back('\n');
return FileUtil::writeBinaryAtomic(missions_path_, body);
}
nlohmann::json MissionService::snapshot() const
{
std::lock_guard lock(mu_);
return doc_;
}
nlohmann::json MissionService::runnerStatus() const
{
std::lock_guard lock(mu_);
return nlohmann::json{{"mode", mode_},
{"state_id", state_id_},
{"queue", queue_},
{"current", current_ ? *current_ : nlohmann::json(nullptr)},
{"action_log", action_log_}};
}
nlohmann::json MissionService::listMissions() const
{
std::lock_guard lock(mu_);
return doc_;
}
std::optional<nlohmann::json> MissionService::getMission(const std::string& id) const
{
std::lock_guard lock(mu_);
for (const auto& m : doc_["missions"])
{
if (m.contains("id") && m["id"].get<std::string>() == id)
return m;
}
return std::nullopt;
}
nlohmann::json MissionService::findMissionUnlocked(const std::string& id) const
{
for (const auto& m : doc_["missions"])
{
if (m.contains("id") && m["id"].get<std::string>() == id)
return m;
}
return nlohmann::json();
}
nlohmann::json MissionService::createMission(const nlohmann::json& payload, std::string& err)
{
std::lock_guard lock(mu_);
if (!payload.is_object() || !payload.contains("name") || !payload["name"].is_string())
{
err = "name is required";
return {};
}
const std::string name = StringUtil::trimCopy(payload["name"].get<std::string>());
if (name.empty())
{
err = "name is required";
return {};
}
for (const auto& m : doc_["missions"])
{
if (m.contains("name") && m["name"].get<std::string>() == name)
{
err = "mission name already exists";
return {};
}
}
nlohmann::json mission = payload;
if (payload.contains("id") && payload["id"].is_string() && !payload["id"].get<std::string>().empty())
mission["id"] = payload["id"].get<std::string>();
else
mission["id"] = IdUtil::newId();
mission["name"] = name;
if (!mission.contains("group") || !mission["group"].is_string())
mission["group"] = "Missions";
if (!mission.contains("description"))
mission["description"] = "";
if (!mission.contains("actions") || !mission["actions"].is_array())
mission["actions"] = nlohmann::json::array();
mission["updated_at"] = IdUtil::nowIso8601();
const std::string group = mission["group"].get<std::string>();
if (std::find(doc_["groups"].begin(), doc_["groups"].end(), group) == doc_["groups"].end())
doc_["groups"].push_back(group);
doc_["missions"].push_back(mission);
return mission;
}
nlohmann::json MissionService::updateMission(const std::string& id, const nlohmann::json& payload, std::string& err)
{
std::lock_guard lock(mu_);
for (auto& m : doc_["missions"])
{
if (!m.contains("id") || m["id"].get<std::string>() != id)
continue;
if (payload.contains("name") && payload["name"].is_string())
{
const std::string name = StringUtil::trimCopy(payload["name"].get<std::string>());
if (name.empty())
{
err = "name is required";
return {};
}
for (const auto& other : doc_["missions"])
{
if (other.contains("id") && other["id"].get<std::string>() != id && other.contains("name") &&
other["name"].get<std::string>() == name)
{
err = "mission name already exists";
return {};
}
}
m["name"] = name;
}
if (payload.contains("group") && payload["group"].is_string())
{
m["group"] = payload["group"];
const std::string group = m["group"].get<std::string>();
if (std::find(doc_["groups"].begin(), doc_["groups"].end(), group) == doc_["groups"].end())
doc_["groups"].push_back(group);
}
if (payload.contains("description") && payload["description"].is_string())
m["description"] = payload["description"];
if (payload.contains("actions") && payload["actions"].is_array())
m["actions"] = payload["actions"];
m["updated_at"] = IdUtil::nowIso8601();
return m;
}
err = "mission not found";
return {};
}
bool MissionService::deleteMission(const std::string& id, std::string& err)
{
std::lock_guard lock(mu_);
auto& missions = doc_["missions"];
for (auto it = missions.begin(); it != missions.end(); ++it)
{
if (it->contains("id") && (*it)["id"].get<std::string>() == id)
{
missions.erase(it);
return true;
}
}
err = "mission not found";
return false;
}
nlohmann::json MissionService::listQueue() const
{
std::lock_guard lock(mu_);
return queue_;
}
nlohmann::json MissionService::enqueueMission(const nlohmann::json& payload, std::string& err)
{
std::lock_guard lock(mu_);
if (!payload.is_object() || !payload.contains("mission_id") || !payload["mission_id"].is_string())
{
err = "mission_id is required";
return {};
}
const std::string mission_id = payload["mission_id"].get<std::string>();
const auto mission = findMissionUnlocked(mission_id);
if (mission.is_null())
{
err = "mission not found";
return {};
}
if (mode_ == "stop")
{
err = "robot is stopped; cannot queue mission";
return {};
}
if (mode_ != "autonomous")
{
err = "robot must be in autonomous mode to queue missions";
return {};
}
nlohmann::json parameters = nlohmann::json::object();
if (payload.contains("parameters") && payload["parameters"].is_object())
parameters = payload["parameters"];
nlohmann::json entry = {{"id", IdUtil::newId()},
{"queue_num", next_queue_num_++},
{"mission_id", mission_id},
{"mission_name", mission["name"]},
{"parameters", parameters},
{"state", "pending"},
{"created_at", IdUtil::nowIso8601()}};
queue_.push_back(entry);
appendLogUnlocked("info", "Mission queued: " + mission["name"].get<std::string>(), {{"queue_id", entry["id"]}});
cv_.notify_all();
return entry;
}
bool MissionService::dequeueEntry(const std::string& queue_id, std::string& err)
{
std::lock_guard lock(mu_);
for (auto it = queue_.begin(); it != queue_.end(); ++it)
{
if ((*it).contains("id") && (*it)["id"].get<std::string>() == queue_id)
{
if ((*it).value("state", "") == "executing")
{
err = "cannot remove executing mission";
return false;
}
queue_.erase(it);
return true;
}
}
err = "queue entry not found";
return false;
}
bool MissionService::clearQueue(std::string& err)
{
(void)err;
std::lock_guard lock(mu_);
nlohmann::json kept = nlohmann::json::array();
for (auto& e : queue_)
{
if (e.value("state", "") == "executing")
kept.push_back(e);
}
queue_ = kept;
if (state_id_ == 3)
cv_.notify_all();
return true;
}
bool MissionService::reorderQueue(const std::string& queue_id, int new_index, std::string& err)
{
std::lock_guard lock(mu_);
if (new_index < 0)
{
err = "invalid index";
return false;
}
nlohmann::json item;
int found = -1;
for (size_t i = 0; i < queue_.size(); ++i)
{
if (queue_[i].contains("id") && queue_[i]["id"].get<std::string>() == queue_id)
{
if (queue_[i].value("state", "") != "pending")
{
err = "only pending missions can be reordered";
return false;
}
item = queue_[i];
found = static_cast<int>(i);
break;
}
}
if (found < 0)
{
err = "queue entry not found";
return false;
}
queue_.erase(queue_.begin() + found);
if (new_index > static_cast<int>(queue_.size()))
new_index = static_cast<int>(queue_.size());
queue_.insert(queue_.begin() + new_index, item);
return true;
}
bool MissionService::setMode(const std::string& mode, std::string& err)
{
if (mode != "autonomous" && mode != "manual" && mode != "stop")
{
err = "mode must be autonomous, manual, or stop";
return false;
}
std::lock_guard lock(mu_);
mode_ = mode;
if (mode == "stop")
state_id_ = 4;
appendLogUnlocked("info", "Operating mode: " + mode);
cv_.notify_all();
return true;
}
bool MissionService::setRunnerState(int state_id, std::string& err)
{
if (state_id != 3 && state_id != 4)
{
err = "state_id must be 3 (play) or 4 (pause)";
return false;
}
std::lock_guard lock(mu_);
state_id_ = state_id;
runner_paused_ = (state_id == 4);
appendLogUnlocked("info", state_id == 3 ? "Runner play" : "Runner pause");
cv_.notify_all();
return true;
}
nlohmann::json MissionService::listTriggers() const
{
std::lock_guard lock(mu_);
return doc_["triggers"];
}
nlohmann::json MissionService::createTrigger(const nlohmann::json& payload, std::string& err)
{
std::lock_guard lock(mu_);
if (!payload.is_object() || !payload.contains("name") || !payload.contains("coil_id") ||
!payload.contains("mission_id"))
{
err = "name, coil_id, mission_id required";
return {};
}
const int coil_id = payload["coil_id"].get<int>();
if (coil_id < 1001 || coil_id > 2000)
{
err = "coil_id must be between 1001 and 2000";
return {};
}
for (const auto& t : doc_["triggers"])
{
if (t.contains("coil_id") && t["coil_id"].get<int>() == coil_id)
{
err = "coil_id already used";
return {};
}
}
const std::string mission_id = payload["mission_id"].get<std::string>();
if (findMissionUnlocked(mission_id).is_null())
{
err = "mission not found";
return {};
}
nlohmann::json trigger = {{"id", IdUtil::newId()},
{"name", StringUtil::trimCopy(payload["name"].get<std::string>())},
{"coil_id", coil_id},
{"mission_id", mission_id}};
doc_["triggers"].push_back(trigger);
return trigger;
}
bool MissionService::deleteTrigger(const std::string& id, std::string& err)
{
std::lock_guard lock(mu_);
auto& triggers = doc_["triggers"];
for (auto it = triggers.begin(); it != triggers.end(); ++it)
{
if (it->contains("id") && (*it)["id"].get<std::string>() == id)
{
triggers.erase(it);
return true;
}
}
err = "trigger not found";
return false;
}
nlohmann::json MissionService::fireTriggerByCoil(int coil_id, std::string& err)
{
nlohmann::json trigger;
std::string mission_id;
{
std::lock_guard lock(mu_);
for (const auto& t : doc_["triggers"])
{
if (t.contains("coil_id") && t["coil_id"].get<int>() == coil_id)
{
trigger = t;
mission_id = t["mission_id"].get<std::string>();
break;
}
}
}
if (mission_id.empty())
{
err = "trigger not found for coil";
return {};
}
return enqueueMission(nlohmann::json{{"mission_id", mission_id}, {"parameters", nlohmann::json::object()}}, err);
}
nlohmann::json MissionService::getDashboard() const
{
std::lock_guard lock(mu_);
return doc_["dashboard"];
}
bool MissionService::setDashboard(const nlohmann::json& payload, std::string& err)
{
if (!payload.is_object() || !payload.contains("widgets") || !payload["widgets"].is_array())
{
err = "widgets array required";
return false;
}
std::lock_guard lock(mu_);
doc_["dashboard"] = payload;
return true;
}
void MissionService::appendLogUnlocked(const std::string& level,
const std::string& message,
const nlohmann::json& extra)
{
nlohmann::json row = {{"ts", IdUtil::nowIso8601()}, {"level", level}, {"message", message}};
if (!extra.empty())
row["extra"] = extra;
action_log_.push_back(row);
while (action_log_.size() > kLogMax)
action_log_.erase(action_log_.begin());
}
nlohmann::json MissionService::paramValue(const nlohmann::json& v)
{
if (v.is_object() && v.contains("kind") && v["kind"].get<std::string>() == "variable")
return v.value("default", "");
return v;
}
void MissionService::resolveParamsUnlocked(nlohmann::json& params, const nlohmann::json& runtime) const
{
if (!params.is_object())
return;
for (auto it = params.begin(); it != params.end(); ++it)
{
if (it.value().is_object() && it.value().contains("kind") &&
it.value()["kind"].get<std::string>() == "variable")
{
const std::string key = it.key();
if (runtime.contains(key))
params[key] = runtime[key];
else
params[key] = it.value().value("default", "");
}
}
}
void MissionService::waitWhilePausedUnlocked()
{
while (!stop_thread_ && (state_id_ == 4 || runner_paused_))
{
mu_.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
mu_.lock();
}
}
void MissionService::sleepMs(int ms)
{
if (ms <= 0)
return;
mu_.unlock();
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
mu_.lock();
}
void MissionService::workerLoop()
{
while (!stop_thread_)
{
{
std::unique_lock lock(mu_);
cv_.wait_for(lock, std::chrono::milliseconds(200), [this] {
if (stop_thread_)
return true;
if (mode_ != "autonomous" || state_id_ != 3)
return false;
if (current_)
return true;
for (const auto& e : queue_)
{
if (e.value("state", "") == "pending")
return true;
}
return false;
});
if (stop_thread_)
break;
tickRunnerUnlocked();
}
}
}
void MissionService::tickRunnerUnlocked()
{
if (mode_ != "autonomous" || state_id_ != 3 || current_)
return;
for (auto& entry : queue_)
{
if (entry.value("state", "") != "pending")
continue;
entry["state"] = "executing";
entry["started_at"] = IdUtil::nowIso8601();
current_ = entry;
executeQueueEntryUnlocked(entry);
entry["state"] = "finished";
entry["finished_at"] = IdUtil::nowIso8601();
appendLogUnlocked("info", "Mission finished: " + entry["mission_name"].get<std::string>());
current_ = std::nullopt;
return;
}
}
void MissionService::executeQueueEntryUnlocked(nlohmann::json& entry)
{
const std::string mission_id = entry["mission_id"].get<std::string>();
const auto mission = findMissionUnlocked(mission_id);
if (mission.is_null())
{
entry["state"] = "failed";
entry["error"] = "mission definition missing";
appendLogUnlocked("error", "Mission missing: " + mission_id);
return;
}
nlohmann::json parameters = entry.value("parameters", nlohmann::json::object());
if (!parameters.is_object())
parameters = nlohmann::json::object();
current_ = entry;
current_->value("progress", nlohmann::json::object())["mission_name"] = mission["name"];
executeActionsUnlocked(mission.value("actions", nlohmann::json::array()), parameters, 0);
}
void MissionService::executeActionsUnlocked(const nlohmann::json& actions,
const nlohmann::json& parameters,
int depth)
{
if (depth > 32)
return;
for (const auto& action : actions)
{
waitWhilePausedUnlocked();
if (stop_thread_ || mode_ != "autonomous")
return;
if (!executeActionUnlocked(action, parameters, depth))
break;
}
}
bool MissionService::executeActionUnlocked(const nlohmann::json& action,
const nlohmann::json& parameters,
int depth)
{
if (!action.is_object())
return true;
const std::string kind = action.value("kind", "action");
if (kind == "mission")
{
const std::string ref = action.value("refId", "");
const auto sub = findMissionUnlocked(ref);
if (sub.is_null())
{
appendLogUnlocked("error", "Embedded mission not found");
return false;
}
appendLogUnlocked("info", "Run sub-mission: " + sub["name"].get<std::string>());
executeActionsUnlocked(sub.value("actions", nlohmann::json::array()), parameters, depth + 1);
return true;
}
const std::string type = action.value("type", "");
nlohmann::json params = action.value("params", nlohmann::json::object());
resolveParamsUnlocked(params, parameters);
if (current_)
{
(*current_)["current_action"] = {{"type", type}, {"label", action.value("label", type)}, {"params", params}};
}
appendLogUnlocked("action", action.value("label", type), {{"type", type}, {"params", params}});
if (type == "pause")
{
runner_paused_ = true;
state_id_ = 4;
appendLogUnlocked("info", "Pause action — waiting for continue");
waitWhilePausedUnlocked();
return true;
}
if (type == "break" || type == "continue")
return type != "break";
if (type == "wait")
{
const int sec = params.value("seconds", 1);
sleepMs(sec * 1000);
return true;
}
if (type == "loop" && action.contains("children") && action["children"].is_array())
{
const std::string loop_mode = params.value("mode", "count");
const int count = params.value("count", 1);
if (loop_mode == "endless")
{
int guard = 0;
while (!stop_thread_ && mode_ == "autonomous" && guard < 1000)
{
waitWhilePausedUnlocked();
executeActionsUnlocked(action["children"], parameters, depth + 1);
++guard;
}
}
else
{
for (int i = 0; i < count && !stop_thread_; ++i)
{
waitWhilePausedUnlocked();
executeActionsUnlocked(action["children"], parameters, depth + 1);
}
}
return true;
}
if (type == "move_to_position" || type == "move_to_marker" || type == "adjust_localization")
sleepMs(1500);
else if (type == "pick_cart" || type == "drop_cart")
sleepMs(2000);
else if (type == "set_digital_output" || type == "wait_digital_input" || type == "set_plc_register")
sleepMs(800);
else if (type == "user_log" || type == "play_sound")
sleepMs(300);
else
sleepMs(500);
return true;
}
} // namespace lm

View File

@@ -0,0 +1,92 @@
#pragma once
#include <nlohmann/json.hpp>
#include <atomic>
#include <condition_variable>
#include <filesystem>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
namespace lm {
class MissionService
{
public:
explicit MissionService(std::filesystem::path data_dir);
~MissionService();
MissionService(const MissionService&) = delete;
MissionService& operator=(const MissionService&) = delete;
nlohmann::json snapshot() const;
nlohmann::json runnerStatus() const;
bool saveDocument() const;
nlohmann::json listMissions() const;
std::optional<nlohmann::json> getMission(const std::string& id) const;
nlohmann::json createMission(const nlohmann::json& payload, std::string& err);
nlohmann::json updateMission(const std::string& id, const nlohmann::json& payload, std::string& err);
bool deleteMission(const std::string& id, std::string& err);
nlohmann::json listQueue() const;
nlohmann::json enqueueMission(const nlohmann::json& payload, std::string& err);
bool dequeueEntry(const std::string& queue_id, std::string& err);
bool clearQueue(std::string& err);
bool reorderQueue(const std::string& queue_id, int new_index, std::string& err);
bool setMode(const std::string& mode, std::string& err);
bool setRunnerState(int state_id, std::string& err);
nlohmann::json listTriggers() const;
nlohmann::json createTrigger(const nlohmann::json& payload, std::string& err);
bool deleteTrigger(const std::string& id, std::string& err);
nlohmann::json fireTriggerByCoil(int coil_id, std::string& err);
nlohmann::json getDashboard() const;
bool setDashboard(const nlohmann::json& payload, std::string& err);
private:
std::filesystem::path data_dir_;
std::filesystem::path missions_path_;
mutable std::mutex mu_;
nlohmann::json doc_;
nlohmann::json queue_;
nlohmann::json action_log_;
std::string mode_{"autonomous"};
int state_id_{3};
std::optional<nlohmann::json> current_;
int next_queue_num_{1};
std::atomic<bool> stop_thread_{false};
std::atomic<bool> runner_paused_{false};
std::condition_variable cv_;
std::thread worker_;
void load();
void ensureDocSchema();
nlohmann::json findMissionUnlocked(const std::string& id) const;
void workerLoop();
void tickRunnerUnlocked();
void executeQueueEntryUnlocked(nlohmann::json& entry);
void executeActionsUnlocked(const nlohmann::json& actions,
const nlohmann::json& parameters,
int depth);
bool executeActionUnlocked(const nlohmann::json& action,
const nlohmann::json& parameters,
int depth);
void appendLogUnlocked(const std::string& level,
const std::string& message,
const nlohmann::json& extra = nlohmann::json::object());
void resolveParamsUnlocked(nlohmann::json& params, const nlohmann::json& runtime) const;
static nlohmann::json paramValue(const nlohmann::json& v);
void waitWhilePausedUnlocked();
void sleepMs(int ms);
};
} // namespace lm