284 lines
8.2 KiB
C++
284 lines
8.2 KiB
C++
#include "robot/robot_runtime.hpp"
|
|
|
|
#include "mission/mission_queue.hpp"
|
|
#include "storage/database.hpp"
|
|
#include "util/id_util.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
|
|
namespace lm {
|
|
|
|
namespace {
|
|
|
|
constexpr const char* kDefaultMessage = "Waiting for new missions...";
|
|
|
|
} // namespace
|
|
|
|
RobotRuntime::RobotRuntime(Database& db, MissionQueue& mission_queue)
|
|
: db_(db), mission_queue_(mission_queue)
|
|
{
|
|
load();
|
|
ensureDefaultsUnlocked();
|
|
}
|
|
|
|
void RobotRuntime::load()
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
state_ = nlohmann::json::object();
|
|
nlohmann::json parsed;
|
|
if (db_.getDocument("robot_runtime", parsed) && parsed.is_object())
|
|
state_ = parsed;
|
|
}
|
|
|
|
void RobotRuntime::saveUnlocked() const
|
|
{
|
|
db_.setDocument("robot_runtime", state_);
|
|
}
|
|
|
|
void RobotRuntime::ensureDefaultsUnlocked()
|
|
{
|
|
if (!state_.is_object())
|
|
state_ = nlohmann::json::object();
|
|
if (!state_.contains("motion"))
|
|
state_["motion"] = "paused";
|
|
if (!state_.contains("health"))
|
|
state_["health"] = "ok";
|
|
if (!state_.contains("message"))
|
|
state_["message"] = kDefaultMessage;
|
|
if (!state_.contains("error"))
|
|
state_["error"] = nullptr;
|
|
if (!state_.contains("battery_percent"))
|
|
state_["battery_percent"] = 54;
|
|
if (!state_.contains("battery_charging"))
|
|
state_["battery_charging"] = false;
|
|
if (!state_.contains("joystick_engaged"))
|
|
state_["joystick_engaged"] = false;
|
|
if (!state_.contains("joystick_speed"))
|
|
state_["joystick_speed"] = "fast";
|
|
if (!state_.contains("cmd_linear"))
|
|
state_["cmd_linear"] = 0.0;
|
|
if (!state_.contains("cmd_angular"))
|
|
state_["cmd_angular"] = 0.0;
|
|
if (!state_.contains("updated_at"))
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
if (!state_.contains("active_map_id"))
|
|
state_["active_map_id"] = nullptr;
|
|
if (!state_.contains("pose") || !state_["pose"].is_object())
|
|
state_["pose"] = {{"x", 0.0}, {"y", 0.0}, {"yaw", 0.0}};
|
|
saveUnlocked();
|
|
}
|
|
|
|
nlohmann::json RobotRuntime::buildStatusUnlocked() const
|
|
{
|
|
const auto runner = mission_queue_.runnerStatus();
|
|
const auto queue = mission_queue_.list();
|
|
|
|
int pending = 0;
|
|
if (queue.is_array())
|
|
{
|
|
for (const auto& item : queue)
|
|
{
|
|
if (item.value("status", "") == "pending")
|
|
++pending;
|
|
}
|
|
}
|
|
|
|
const std::string motion = state_.value("motion", "paused");
|
|
const std::string health = state_.value("health", "ok");
|
|
const std::string runner_state = runner.value("state", "idle");
|
|
|
|
std::string message = state_.value("message", kDefaultMessage);
|
|
if (health == "ok")
|
|
{
|
|
if (!runner.value("message", "").empty())
|
|
message = runner.value("message", "");
|
|
else if (runner_state == "idle" && motion == "paused" && pending == 0)
|
|
message = kDefaultMessage;
|
|
}
|
|
|
|
nlohmann::json body = {{"motion", motion},
|
|
{"health", health},
|
|
{"message", message},
|
|
{"error", state_.contains("error") ? state_["error"] : nullptr},
|
|
{"battery_percent", state_.value("battery_percent", 54)},
|
|
{"battery_charging", state_.value("battery_charging", false)},
|
|
{"joystick_engaged", state_.value("joystick_engaged", false)},
|
|
{"joystick_speed", state_.value("joystick_speed", "fast")},
|
|
{"cmd_linear", state_.value("cmd_linear", 0.0)},
|
|
{"cmd_angular", state_.value("cmd_angular", 0.0)},
|
|
{"active_map_id", state_.contains("active_map_id") ? state_["active_map_id"] : nullptr},
|
|
{"pose", state_.contains("pose") ? state_["pose"] : nlohmann::json::object({{"x", 0.0}, {"y", 0.0}, {"yaw", 0.0}})},
|
|
{"runner", runner},
|
|
{"queue_pending", pending},
|
|
{"updated_at", state_.value("updated_at", "")}};
|
|
return body;
|
|
}
|
|
|
|
nlohmann::json RobotRuntime::status() const
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
return buildStatusUnlocked();
|
|
}
|
|
|
|
bool RobotRuntime::start(std::string& err)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
if (state_.value("health", "ok") == "error")
|
|
{
|
|
err = "cannot start while robot is in error state — reset error first";
|
|
return false;
|
|
}
|
|
|
|
state_["motion"] = "running";
|
|
state_["message"] = "Robot running";
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
|
|
const std::string runner_state = mission_queue_.runnerStatus().value("state", "idle");
|
|
if (runner_state == "paused")
|
|
return mission_queue_.resume(err);
|
|
return true;
|
|
}
|
|
|
|
bool RobotRuntime::pause(std::string& err)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
state_["motion"] = "paused";
|
|
state_["message"] = "Robot paused";
|
|
state_["joystick_engaged"] = false;
|
|
state_["cmd_linear"] = 0.0;
|
|
state_["cmd_angular"] = 0.0;
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
|
|
const std::string runner_state = mission_queue_.runnerStatus().value("state", "idle");
|
|
if (runner_state == "running" || runner_state == "paused")
|
|
return mission_queue_.pause(err);
|
|
return true;
|
|
}
|
|
|
|
bool RobotRuntime::resetError(std::string& err)
|
|
{
|
|
(void)err;
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
state_["health"] = "ok";
|
|
state_["error"] = nullptr;
|
|
state_["message"] = kDefaultMessage;
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool RobotRuntime::setCmdVel(double linear, double angular, std::string& err)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
if (state_.value("health", "ok") == "error")
|
|
{
|
|
err = "robot is in error state";
|
|
return false;
|
|
}
|
|
if (!state_.value("joystick_engaged", false))
|
|
{
|
|
err = "joystick is not engaged";
|
|
return false;
|
|
}
|
|
if (state_.value("motion", "paused") != "running")
|
|
{
|
|
err = "robot motion is paused — press START first";
|
|
return false;
|
|
}
|
|
|
|
const std::string speed = state_.value("joystick_speed", "fast");
|
|
double scale = 1.0;
|
|
if (speed == "medium")
|
|
scale = 0.55;
|
|
else if (speed == "slow")
|
|
scale = 0.25;
|
|
|
|
linear = std::clamp(linear, -1.0, 1.0) * scale;
|
|
angular = std::clamp(angular, -1.0, 1.0) * scale;
|
|
|
|
state_["cmd_linear"] = linear;
|
|
state_["cmd_angular"] = angular;
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool RobotRuntime::setJoystick(bool engaged, const std::string& speed, std::string& err)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
if (engaged && state_.value("health", "ok") == "error")
|
|
{
|
|
err = "cannot engage joystick while robot is in error state";
|
|
return false;
|
|
}
|
|
if (engaged && state_.value("motion", "paused") != "running")
|
|
{
|
|
err = "start the robot before engaging the joystick";
|
|
return false;
|
|
}
|
|
|
|
state_["joystick_engaged"] = engaged;
|
|
if (!speed.empty())
|
|
{
|
|
if (speed != "fast" && speed != "medium" && speed != "slow")
|
|
{
|
|
err = "speed must be fast, medium, or slow";
|
|
return false;
|
|
}
|
|
state_["joystick_speed"] = speed;
|
|
}
|
|
if (!engaged)
|
|
{
|
|
state_["cmd_linear"] = 0.0;
|
|
state_["cmd_angular"] = 0.0;
|
|
}
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
bool RobotRuntime::setActiveMap(const std::string& map_id, std::string& err)
|
|
{
|
|
(void)err;
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
state_["active_map_id"] = map_id;
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
return true;
|
|
}
|
|
|
|
void RobotRuntime::clearActiveMapIf(const std::string& map_id)
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
if (state_.value("active_map_id", "") == map_id)
|
|
{
|
|
state_["active_map_id"] = nullptr;
|
|
state_["updated_at"] = IdUtil::nowIso8601();
|
|
saveUnlocked();
|
|
}
|
|
}
|
|
|
|
void RobotRuntime::tick()
|
|
{
|
|
std::lock_guard<std::mutex> lock(mu_);
|
|
const bool running = state_.value("motion", "paused") == "running";
|
|
const bool joy = state_.value("joystick_engaged", false);
|
|
double battery = state_.value("battery_percent", 54.0);
|
|
|
|
if (running && (joy || std::abs(state_.value("cmd_linear", 0.0)) > 0.01))
|
|
battery = std::max(0.0, battery - 0.02);
|
|
else if (state_.value("battery_charging", false))
|
|
battery = std::min(100.0, battery + 0.05);
|
|
else if (!running)
|
|
battery = std::min(100.0, battery + 0.005);
|
|
|
|
state_["battery_percent"] = static_cast<int>(std::lround(battery));
|
|
saveUnlocked();
|
|
}
|
|
|
|
} // namespace lm
|