add top bar

src/robot/robot_runtime.cpp

@@ -0,0 +1,265 @@
+#include "robot/robot_runtime.hpp"
+
+#include "mission/mission_queue.hpp"
+#include "util/file_util.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(std::filesystem::path runtime_path, MissionQueue& mission_queue)
+    : runtime_path_(std::move(runtime_path)), mission_queue_(mission_queue)
+{
+  load();
+  ensureDefaultsUnlocked();
+}
+
+void RobotRuntime::load()
+{
+  std::lock_guard<std::mutex> lock(mu_);
+  state_ = nlohmann::json::object();
+  if (!std::filesystem::exists(runtime_path_))
+    return;
+  try
+  {
+    const auto parsed = nlohmann::json::parse(FileUtil::readBinary(runtime_path_));
+    if (parsed.is_object())
+      state_ = parsed;
+  }
+  catch (...)
+  {
+    state_ = nlohmann::json::object();
+  }
+}
+
+void RobotRuntime::saveUnlocked() const
+{
+  FileUtil::writeBinaryAtomic(runtime_path_, state_.dump(2));
+}
+
+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();
+  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)},
+                         {"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;
+}
+
+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