770 lines
23 KiB
C++
770 lines
23 KiB
C++
#include "mission/mission_queue.hpp"
|
|
|
|
#include "mission/mission_store.hpp"
|
|
#include "mission/position_resolver.hpp"
|
|
#include "storage/database.hpp"
|
|
#include "storage/map_store.hpp"
|
|
#include "storage/transition_store.hpp"
|
|
#include "util/id_util.hpp"
|
|
|
|
#include <chrono>
|
|
#include <stdexcept>
|
|
#include <thread>
|
|
#include <unordered_set>
|
|
|
|
namespace lm {
|
|
|
|
namespace {
|
|
|
|
class MissionCancelled : public std::runtime_error
|
|
{
|
|
public:
|
|
MissionCancelled() : std::runtime_error("mission cancelled") {}
|
|
};
|
|
|
|
std::string paramValue(const std::string& action_id,
|
|
const nlohmann::json& params,
|
|
const std::string& key,
|
|
const nlohmann::json& parameters)
|
|
{
|
|
const std::string lookup = action_id + ":" + key;
|
|
if (parameters.is_object() && parameters.contains(lookup))
|
|
return parameters[lookup].get<std::string>();
|
|
if (params.contains(key) && params[key].is_string())
|
|
return params[key].get<std::string>();
|
|
return "";
|
|
}
|
|
|
|
double paramNumber(const nlohmann::json& params, const std::string& key, double fallback)
|
|
{
|
|
if (params.contains(key) && params[key].is_number())
|
|
return params[key].get<double>();
|
|
return fallback;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
MissionQueue::MissionQueue(Database& db, MapStore& maps, TransitionStore& transitions, MissionStore& missions)
|
|
: db_(db), maps_(maps), transitions_(transitions), missions_(missions), position_resolver_(maps)
|
|
{
|
|
load();
|
|
ensureRunnerDefaults();
|
|
startWorkerIfNeeded();
|
|
}
|
|
|
|
MissionQueue::~MissionQueue()
|
|
{
|
|
stop_ = true;
|
|
wake_ = true;
|
|
if (worker_.joinable())
|
|
worker_.join();
|
|
}
|
|
|
|
void MissionQueue::load()
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
queue_ = nlohmann::json::array();
|
|
runner_ = nlohmann::json::object();
|
|
nlohmann::json parsed;
|
|
if (!db_.getDocument("mission_queue", parsed))
|
|
return;
|
|
if (parsed.is_object())
|
|
{
|
|
if (parsed.contains("queue") && parsed["queue"].is_array())
|
|
queue_ = parsed["queue"];
|
|
if (parsed.contains("runner") && parsed["runner"].is_object())
|
|
runner_ = parsed["runner"];
|
|
}
|
|
ensureRunnerDefaults();
|
|
}
|
|
|
|
void MissionQueue::saveUnlocked() const
|
|
{
|
|
const nlohmann::json payload = {{"queue", queue_}, {"runner", runner_}};
|
|
db_.setDocument("mission_queue", payload);
|
|
}
|
|
|
|
void MissionQueue::ensureRunnerDefaults()
|
|
{
|
|
if (!runner_.is_object())
|
|
runner_ = nlohmann::json::object();
|
|
if (!runner_.contains("state"))
|
|
runner_["state"] = "idle";
|
|
if (!runner_.contains("message"))
|
|
runner_["message"] = "";
|
|
if (!runner_.contains("current_queue_id"))
|
|
runner_["current_queue_id"] = nullptr;
|
|
if (!runner_.contains("current_action"))
|
|
runner_["current_action"] = nullptr;
|
|
if (!runner_.contains("paused"))
|
|
runner_["paused"] = false;
|
|
}
|
|
|
|
void MissionQueue::startWorkerIfNeeded()
|
|
{
|
|
if (worker_.joinable())
|
|
return;
|
|
worker_ = std::thread([this] { workerLoop(); });
|
|
}
|
|
|
|
nlohmann::json MissionQueue::list() const
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
return queue_;
|
|
}
|
|
|
|
nlohmann::json MissionQueue::runnerStatus() const
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
return runner_;
|
|
}
|
|
|
|
std::optional<nlohmann::json> MissionQueue::enqueue(const nlohmann::json& payload, std::string& err)
|
|
{
|
|
if (!payload.is_object())
|
|
{
|
|
err = "payload must be an object";
|
|
return std::nullopt;
|
|
}
|
|
if (!payload.contains("mission") || !payload["mission"].is_object())
|
|
{
|
|
err = "mission is required";
|
|
return std::nullopt;
|
|
}
|
|
|
|
nlohmann::json entry = nlohmann::json::object();
|
|
entry["id"] = IdUtil::newId();
|
|
entry["mission_id"] = payload["mission"].value("id", "");
|
|
entry["mission_name"] = payload["mission"].value("name", "Mission");
|
|
entry["mission_group"] = payload["mission"].value("group", "Missions");
|
|
entry["mission"] = payload["mission"];
|
|
entry["parameters"] = payload.contains("parameters") && payload["parameters"].is_object() ? payload["parameters"]
|
|
: nlohmann::json::object();
|
|
entry["priority"] = payload.contains("priority") && payload["priority"].is_number() ? payload["priority"].get<int>() : 0;
|
|
entry["robot_id"] = payload.value("robot_id", "default");
|
|
entry["source"] = payload.value("source", "ui");
|
|
entry["status"] = "pending";
|
|
entry["created_at"] = IdUtil::nowIso8601();
|
|
entry["started_at"] = nullptr;
|
|
entry["finished_at"] = nullptr;
|
|
entry["log"] = nlohmann::json::array();
|
|
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
insertByPriorityUnlocked(entry);
|
|
saveUnlocked();
|
|
}
|
|
|
|
wake_ = true;
|
|
return entry;
|
|
}
|
|
|
|
void MissionQueue::insertByPriorityUnlocked(nlohmann::json& entry)
|
|
{
|
|
const int priority = entry.value("priority", 0);
|
|
size_t insert_at = queue_.size();
|
|
for (size_t i = 0; i < queue_.size(); ++i)
|
|
{
|
|
if (!queue_[i].is_object())
|
|
continue;
|
|
if (queue_[i].value("status", "") != "pending")
|
|
continue;
|
|
const int existing = queue_[i].value("priority", 0);
|
|
if (priority > existing)
|
|
{
|
|
insert_at = i;
|
|
break;
|
|
}
|
|
}
|
|
if (insert_at >= queue_.size())
|
|
queue_.push_back(entry);
|
|
else
|
|
queue_.insert(queue_.begin() + static_cast<nlohmann::json::difference_type>(insert_at), entry);
|
|
}
|
|
|
|
bool MissionQueue::removeById(const std::string& id, std::string& err)
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
if (!queue_.is_array())
|
|
{
|
|
err = "queue unavailable";
|
|
return false;
|
|
}
|
|
const auto before = queue_.size();
|
|
nlohmann::json next = nlohmann::json::array();
|
|
for (const auto& item : queue_)
|
|
{
|
|
if (!item.is_object())
|
|
continue;
|
|
if (item.value("id", "") == id)
|
|
{
|
|
const std::string status = item.value("status", "");
|
|
if (status == "executing")
|
|
{
|
|
err = "cannot remove executing mission";
|
|
return false;
|
|
}
|
|
continue;
|
|
}
|
|
next.push_back(item);
|
|
}
|
|
if (next.size() == before)
|
|
{
|
|
err = "queue item not found";
|
|
return false;
|
|
}
|
|
queue_ = std::move(next);
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool MissionQueue::clearAll(std::string& err)
|
|
{
|
|
(void)err;
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
nlohmann::json next = nlohmann::json::array();
|
|
for (const auto& item : queue_)
|
|
{
|
|
if (!item.is_object())
|
|
continue;
|
|
if (item.value("status", "") == "executing")
|
|
next.push_back(item);
|
|
}
|
|
queue_ = std::move(next);
|
|
setRunnerState(next.empty() ? "idle" : "running", next.empty() ? "" : "Đang thực thi mission");
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool MissionQueue::reorder(const nlohmann::json& ordered_ids, std::string& err)
|
|
{
|
|
if (!ordered_ids.is_array())
|
|
{
|
|
err = "ordered_ids must be an array";
|
|
return false;
|
|
}
|
|
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
if (!queue_.is_array())
|
|
{
|
|
err = "queue unavailable";
|
|
return false;
|
|
}
|
|
|
|
nlohmann::json by_id = nlohmann::json::object();
|
|
for (auto& item : queue_)
|
|
{
|
|
if (item.is_object())
|
|
by_id[item.value("id", "")] = item;
|
|
}
|
|
|
|
nlohmann::json next = nlohmann::json::array();
|
|
std::unordered_set<std::string> seen;
|
|
for (const auto& id_json : ordered_ids)
|
|
{
|
|
if (!id_json.is_string())
|
|
continue;
|
|
const std::string id = id_json.get<std::string>();
|
|
if (!by_id.contains(id))
|
|
continue;
|
|
next.push_back(by_id[id]);
|
|
seen.insert(id);
|
|
}
|
|
|
|
for (const auto& item : queue_)
|
|
{
|
|
if (!item.is_object())
|
|
continue;
|
|
const std::string id = item.value("id", "");
|
|
if (seen.count(id))
|
|
continue;
|
|
next.push_back(item);
|
|
}
|
|
|
|
queue_ = std::move(next);
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool MissionQueue::pause(std::string& err)
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
const std::string state = runner_.value("state", "idle");
|
|
if (state != "running")
|
|
{
|
|
err = "no mission is running";
|
|
return false;
|
|
}
|
|
paused_ = true;
|
|
runner_["paused"] = true;
|
|
runner_["state"] = "paused";
|
|
runner_["message"] = "Mission tạm dừng";
|
|
runner_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool MissionQueue::resume(std::string& err)
|
|
{
|
|
(void)err;
|
|
paused_ = false;
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
runner_["paused"] = false;
|
|
if (runner_.value("state", "") == "paused")
|
|
{
|
|
runner_["state"] = "running";
|
|
runner_["message"] = "Tiếp tục mission";
|
|
runner_["updated_at"] = IdUtil::nowIso8601();
|
|
}
|
|
saveUnlocked();
|
|
}
|
|
wake_ = true;
|
|
return true;
|
|
}
|
|
|
|
bool MissionQueue::cancel(std::string& err)
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
const std::string state = runner_.value("state", "idle");
|
|
if (state != "running" && state != "paused")
|
|
{
|
|
err = "no mission is running";
|
|
return false;
|
|
}
|
|
if (cancel_)
|
|
{
|
|
err = "cancel already in progress";
|
|
return false;
|
|
}
|
|
cancel_ = true;
|
|
paused_ = false;
|
|
runner_["paused"] = false;
|
|
runner_["message"] = "Đang hủy mission…";
|
|
runner_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
wake_ = true;
|
|
return true;
|
|
}
|
|
|
|
void MissionQueue::workerLoop()
|
|
{
|
|
while (!stop_)
|
|
{
|
|
nlohmann::json working;
|
|
bool run = false;
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
if (!paused_ && queue_.is_array())
|
|
{
|
|
for (auto& item : queue_)
|
|
{
|
|
if (!item.is_object())
|
|
continue;
|
|
if (item.value("status", "") != "pending")
|
|
continue;
|
|
item["status"] = "executing";
|
|
item["started_at"] = IdUtil::nowIso8601();
|
|
runner_["current_queue_id"] = item.value("id", "");
|
|
runner_["current_action"] = nullptr;
|
|
setRunnerState("running", "Đang chạy: " + item.value("mission_name", "Mission"));
|
|
saveUnlocked();
|
|
working = item;
|
|
run = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!run && runner_.value("state", "") == "running")
|
|
{
|
|
setRunnerState("idle", "Queue trống");
|
|
saveUnlocked();
|
|
}
|
|
}
|
|
|
|
if (run)
|
|
{
|
|
runMissionActions(working);
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
const std::string id = working.value("id", "");
|
|
for (auto& item : queue_)
|
|
{
|
|
if (item.is_object() && item.value("id", "") == id)
|
|
{
|
|
item = working;
|
|
break;
|
|
}
|
|
}
|
|
runner_["current_queue_id"] = nullptr;
|
|
runner_["current_action"] = nullptr;
|
|
saveUnlocked();
|
|
}
|
|
wake_ = true;
|
|
}
|
|
|
|
for (int i = 0; i < 20 && !wake_; ++i)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
wake_ = false;
|
|
}
|
|
}
|
|
|
|
void MissionQueue::runMissionActions(nlohmann::json& entry)
|
|
{
|
|
cancel_ = false;
|
|
nlohmann::json log = nlohmann::json::array();
|
|
try
|
|
{
|
|
const auto& mission = entry["mission"];
|
|
const auto& parameters = entry["parameters"];
|
|
const auto& actions =
|
|
mission.contains("actions") && mission["actions"].is_array() ? mission["actions"] : nlohmann::json::array();
|
|
executeActionsUnlocked(actions, parameters, log, 0);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
entry["log"] = log;
|
|
entry["status"] = "completed";
|
|
entry["finished_at"] = IdUtil::nowIso8601();
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
setRunnerState("idle", "Hoàn thành: " + entry.value("mission_name", "Mission"));
|
|
saveUnlocked();
|
|
}
|
|
}
|
|
catch (const MissionCancelled&)
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "warn"}, {"message", "Mission hủy bởi operator"}});
|
|
entry["log"] = log;
|
|
entry["status"] = "cancelled";
|
|
entry["finished_at"] = IdUtil::nowIso8601();
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
setRunnerState("idle", "Đã hủy: " + entry.value("mission_name", "Mission"));
|
|
saveUnlocked();
|
|
}
|
|
}
|
|
catch (...)
|
|
{
|
|
entry["status"] = "failed";
|
|
entry["finished_at"] = IdUtil::nowIso8601();
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
setRunnerState("error", "Lỗi khi chạy: " + entry.value("mission_name", "Mission"));
|
|
saveUnlocked();
|
|
}
|
|
}
|
|
cancel_ = false;
|
|
}
|
|
|
|
void MissionQueue::runAutoTransitionIfNeeded(const std::string& from_map_id,
|
|
const std::string& to_map_id,
|
|
const nlohmann::json& parameters,
|
|
nlohmann::json& log,
|
|
int loop_depth)
|
|
{
|
|
if (from_map_id.empty() || to_map_id.empty() || from_map_id == to_map_id)
|
|
return;
|
|
|
|
const auto transition = transitions_.findBetween(from_map_id, to_map_id);
|
|
if (!transition)
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "error"},
|
|
{"message", "No transition configured: " + from_map_id + " → " + to_map_id}});
|
|
throw std::runtime_error("no transition between maps");
|
|
}
|
|
|
|
std::string from_label = from_map_id;
|
|
std::string to_label = to_map_id;
|
|
const auto maps = maps_.list();
|
|
if (maps.is_array())
|
|
{
|
|
for (const auto& map : maps)
|
|
{
|
|
if (!map.is_object())
|
|
continue;
|
|
if (map.value("id", "") == from_map_id)
|
|
from_label = map.value("name", from_map_id);
|
|
if (map.value("id", "") == to_map_id)
|
|
to_label = map.value("name", to_map_id);
|
|
}
|
|
}
|
|
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", "Auto transition: " + from_label + " → " + to_label}});
|
|
|
|
const std::string start_id = transition->value("start_position_id", "");
|
|
const std::string goal_id = transition->value("goal_position_id", "");
|
|
if (!start_id.empty())
|
|
{
|
|
const auto start_pos = position_resolver_.resolve(start_id, from_map_id);
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message",
|
|
"Move to transition start → " + (start_pos ? start_pos->label : start_id)}});
|
|
sleepMs(1200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
|
|
const std::string mission_id = transition->value("mission_id", "");
|
|
if (!mission_id.empty())
|
|
{
|
|
const auto mission = missions_.findMission(mission_id);
|
|
if (mission && mission->contains("actions") && (*mission)["actions"].is_array())
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", "Run transition mission: " + mission->value("name", mission_id)}});
|
|
executeActionsUnlocked((*mission)["actions"], parameters, log, loop_depth + 1, false);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
else
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "warn"},
|
|
{"message", "Transition mission not found: " + mission_id}});
|
|
}
|
|
}
|
|
|
|
std::string err;
|
|
if (!setActiveMapOnRobotRuntimeDoc(db_, to_map_id, err))
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "error"}, {"message", "Switch map failed: " + err}});
|
|
throw std::runtime_error("switch_map failed after transition");
|
|
}
|
|
|
|
if (!goal_id.empty())
|
|
{
|
|
const auto goal_pos = position_resolver_.resolve(goal_id, to_map_id);
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", "Move to transition goal → " + (goal_pos ? goal_pos->label : goal_id)}});
|
|
sleepMs(1200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
}
|
|
|
|
MissionQueue::LoopControl MissionQueue::executeActionsUnlocked(const nlohmann::json& actions,
|
|
const nlohmann::json& parameters,
|
|
nlohmann::json& log,
|
|
int loop_depth,
|
|
bool allow_auto_transition)
|
|
{
|
|
if (loop_depth > 8)
|
|
throw std::runtime_error("loop depth exceeded");
|
|
|
|
for (const auto& action : actions)
|
|
{
|
|
if (!action.is_object())
|
|
continue;
|
|
while (paused_ && !stop_ && !cancel_)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
if (stop_)
|
|
return LoopControl::None;
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
|
|
const std::string action_id = action.value("id", "");
|
|
const std::string kind = action.value("kind", "action");
|
|
const std::string type = action.value("type", "");
|
|
const std::string label = action.value("label", type);
|
|
const auto& params = action.contains("params") && action["params"].is_object() ? action["params"]
|
|
: nlohmann::json::object();
|
|
|
|
{
|
|
std::lock_guard<std::recursive_mutex> lock(mu_);
|
|
runner_["current_action"] = label;
|
|
saveUnlocked();
|
|
}
|
|
|
|
if (kind == "mission")
|
|
{
|
|
const std::string ref_id = action.value("refId", "");
|
|
nlohmann::json ref_mission = nlohmann::json::object();
|
|
(void)ref_id;
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", "Sub-mission: " + label}});
|
|
if (action.contains("resolved_mission") && action["resolved_mission"].is_object())
|
|
{
|
|
const auto& nested_actions = action["resolved_mission"]["actions"];
|
|
const LoopControl nested = executeActionsUnlocked(nested_actions, parameters, log, loop_depth, allow_auto_transition);
|
|
if (nested == LoopControl::Break)
|
|
return LoopControl::Break;
|
|
if (nested == LoopControl::Continue)
|
|
continue;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (type == "break")
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", "Break loop"}});
|
|
return LoopControl::Break;
|
|
}
|
|
|
|
if (type == "continue")
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", "Continue loop"}});
|
|
return LoopControl::Continue;
|
|
}
|
|
|
|
if (type == "loop")
|
|
{
|
|
const std::string mode = params.value("mode", "count");
|
|
const int count = static_cast<int>(paramNumber(params, "count", 1));
|
|
const auto& children =
|
|
action.contains("children") && action["children"].is_array() ? action["children"] : nlohmann::json::array();
|
|
const int iterations = mode == "endless" ? 10000 : std::max(1, count);
|
|
for (int i = 0; i < iterations && !stop_ && !cancel_; ++i)
|
|
{
|
|
if (mode == "endless" && i == 0)
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", "Loop endless (simulated, max " + std::to_string(iterations) + ")"}});
|
|
}
|
|
else if (mode != "endless")
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", "Loop " + std::to_string(i + 1) + "/" + std::to_string(iterations)}});
|
|
}
|
|
const LoopControl ctrl = executeActionsUnlocked(children, parameters, log, loop_depth + 1, allow_auto_transition);
|
|
if (ctrl == LoopControl::Break)
|
|
break;
|
|
if (ctrl == LoopControl::Continue)
|
|
continue;
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (type == "wait")
|
|
{
|
|
const int ms = static_cast<int>(paramNumber(params, "seconds", 1) * 1000);
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", "Wait " + std::to_string(ms) + "ms"}});
|
|
sleepMs(ms);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
if (type == "move_to_position" || type == "adjust_localization" || type == "pick_cart" || type == "drop_cart")
|
|
{
|
|
std::string pos_ref = paramValue(action_id, params, "position_id", parameters);
|
|
if (pos_ref.empty())
|
|
pos_ref = paramValue(action_id, params, "position", parameters);
|
|
|
|
if (allow_auto_transition && (type == "move_to_position" || type == "adjust_localization") && !pos_ref.empty())
|
|
{
|
|
const std::string active_map = getActiveMapIdFromDb(db_);
|
|
const auto resolved = position_resolver_.resolve(pos_ref, active_map);
|
|
if (resolved)
|
|
{
|
|
if (!active_map.empty() && active_map != resolved->map_id)
|
|
{
|
|
runAutoTransitionIfNeeded(active_map, resolved->map_id, parameters, log, loop_depth);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", label + " → " + resolved->label}});
|
|
sleepMs(1200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
}
|
|
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", label + " → " + (pos_ref.empty() ? "?" : pos_ref)}});
|
|
sleepMs(1200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
if (type == "switch_map")
|
|
{
|
|
const std::string map_id = paramValue(action_id, params, "map_id", parameters);
|
|
std::string err;
|
|
if (!setActiveMapOnRobotRuntimeDoc(db_, map_id, err))
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "error"}, {"message", "Switch map failed: " + err}});
|
|
throw std::runtime_error("switch_map failed");
|
|
}
|
|
log.push_back(
|
|
{{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", "Switch map → " + map_id}});
|
|
sleepMs(300);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
if (type == "move_to_marker")
|
|
{
|
|
const std::string marker = paramValue(action_id, params, "marker", parameters);
|
|
log.push_back({{"ts", IdUtil::nowIso8601()},
|
|
{"level", "info"},
|
|
{"message", label + " → " + (marker.empty() ? "?" : marker)}});
|
|
sleepMs(1200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
if (type == "user_log")
|
|
{
|
|
const std::string message = params.value("message", "Mission step");
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "user"}, {"message", message}});
|
|
sleepMs(200);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
if (type == "pause")
|
|
{
|
|
log.push_back({{"ts", IdUtil::nowIso8601()}, {"level", "warn"}, {"message", "Pause (simulated)"}});
|
|
sleepMs(500);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
continue;
|
|
}
|
|
|
|
log.push_back(
|
|
{{"ts", IdUtil::nowIso8601()}, {"level", "info"}, {"message", label + " (" + type + ") simulated"}});
|
|
sleepMs(400);
|
|
if (cancel_)
|
|
throw MissionCancelled();
|
|
}
|
|
return LoopControl::None;
|
|
}
|
|
|
|
void MissionQueue::sleepMs(int ms)
|
|
{
|
|
if (ms <= 0)
|
|
return;
|
|
const int step = 100;
|
|
for (int elapsed = 0; elapsed < ms && !stop_ && !cancel_; elapsed += step)
|
|
{
|
|
while (paused_ && !stop_ && !cancel_)
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(std::min(step, ms - elapsed)));
|
|
}
|
|
}
|
|
|
|
void MissionQueue::setRunnerState(const std::string& state, const std::string& message)
|
|
{
|
|
runner_["state"] = state;
|
|
runner_["message"] = message;
|
|
runner_["updated_at"] = IdUtil::nowIso8601();
|
|
}
|
|
|
|
} // namespace lm
|