first commit

src/bridges/mission_adapter_bridge.cpp

@@ -0,0 +1,239 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt MissionAdapterBridge.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/bridges/mission_adapter_bridge.h>
+
+#include <utility>
+
+#include <mission_adapters/mission_manager.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+
+MissionAdapterBridge::MissionAdapterBridge() = default;
+MissionAdapterBridge::~MissionAdapterBridge() = default;
+
+void MissionAdapterBridge::attach(mission_adapters::MissionManager* manager)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  manager_ = manager;
+}
+
+void MissionAdapterBridge::setCancelCallback(CancelCallback callback)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  cancel_callback_ = std::move(callback);
+}
+
+void MissionAdapterBridge::setRequestCallback(RequestCallback callback)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  request_callback_ = std::move(callback);
+}
+
+// ================================================================================================
+// Chuyển đổi
+// ================================================================================================
+
+NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Mission& mission)
+{
+  NavigationRequest request;
+
+  request.mission_sequence_id = mission.id;
+  request.has_goal = mission.has_goal;
+  request.goal = mission.goal;
+
+  // Sai số để mặc định: quy ước của NavigationRequest là giá trị <= 0 nghĩa "dùng default của
+  // profile trong config". Mission layer không biết gì về sai số hình học nên không được đặt.
+  request.tolerance = GoalTolerance();
+
+  // Mission layer KHÔNG diễn giải actionType và không lọc gì (D8) — action đi qua nguyên vẹn, đúng
+  // thứ tự đã sắp theo sequenceId.
+  request.actions.reserve(mission.actions.size());
+  for (const auto& action : mission.actions)
+  {
+    request.actions.push_back(action.action);
+  }
+
+  // Mọi mission đều chạy profile position. `MissionType` chỉ nói mission đến TỪ ĐÂU (goal đơn hay
+  // order VDA5050), không nói robot phải di chuyển KIỂU gì — docking/go-straight/rotate là lựa chọn
+  // của người vận hành qua sáu entry point của contract host, không phải của mission layer.
+  request.profile = MotionProfile::kPosition;
+
+  return request;
+}
+
+// ================================================================================================
+// NavigationClient — gọi từ thread của MissionExecutor
+// ================================================================================================
+
+bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission)
+{
+  if (!mission)
+  {
+    robot::log_error("[move_base2] MissionAdapterBridge: dispatch(nullptr).\n");
+    return false;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!running_)
+  {
+    // Chưa start hoặc đã stop. Từ chối thay vì cất lại: mission layer phải biết chặng của nó không
+    // được nhận, chứ không phải chờ một kết quả sẽ không bao giờ tới.
+    robot::log_warning("[move_base2] MissionAdapterBridge: từ chối mission %llu — bridge chưa "
+                       "start.\n", static_cast<unsigned long long>(mission->id));
+    return false;
+  }
+
+  if (pending_)
+  {
+    // Không nên xảy ra: MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Đếm lại thay
+    // vì im lặng — một chặng biến mất trong khi fleet master vẫn chờ nó là lỗi rất khó truy.
+    ++dropped_requests_;
+    robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu đè mission %llu chưa kịp "
+                       "đẩy xuống.\n", static_cast<unsigned long long>(mission->id),
+                       static_cast<unsigned long long>(pending_->id));
+  }
+
+  // Chỉ cất lại. Chuyển đổi và đẩy xuống navigation xảy ra ở pumpPendingRequest(), trên control
+  // thread — xem doc của lớp.
+  pending_ = mission;
+  return true;
+}
+
+void MissionAdapterBridge::cancelActive(mission_adapters::MissionId /*id*/)
+{
+  CancelCallback callback;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    // Chặng đang chờ mà chưa kịp xuống navigation thì huỷ ngay tại đây: đẩy nó xuống rồi mới huỷ là
+    // cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi.
+    pending_.reset();
+    callback = cancel_callback_;
+  }
+
+  // Gọi NGOÀI lock: callback đi vào control loop, không được chạy dưới mutex của bridge.
+  if (callback)
+  {
+    callback();
+  }
+}
+
+// ================================================================================================
+// Biên thread — chỉ control thread gọi
+// ================================================================================================
+
+bool MissionAdapterBridge::pumpPendingRequest()
+{
+  std::shared_ptr<const mission_adapters::Mission> mission;
+  RequestCallback callback;
+
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!pending_ || !request_callback_)
+    {
+      return false;
+    }
+    mission = pending_;
+    pending_.reset();
+    callback = request_callback_;
+  }
+
+  // Ngoài lock: callback đi thẳng vào ControlLoop::submit.
+  callback(toRequest(*mission));
+  return true;
+}
+
+// ================================================================================================
+// MissionPort — gọi từ control thread
+// ================================================================================================
+
+void MissionAdapterBridge::reportOutcome(std::uint64_t mission_sequence_id,
+                                         NavigationOutcome outcome)
+{
+  if (mission_sequence_id == mission_adapters::kInvalidMissionId)
+  {
+    // Goal trực tiếp từ contract host, không thuộc mission nào. Không có gì để báo.
+    return;
+  }
+
+  mission_adapters::MissionManager* manager = nullptr;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    manager = manager_;
+  }
+
+  if (manager == nullptr)
+  {
+    return;
+  }
+
+  // Chỉ kSucceeded mới là "chặng xong". Ba kết cục còn lại đều là "chặng không hoàn thành", và
+  // chính sách hàng đợi (`clear_queue_on_failure`) nằm ở mission layer chứ không ở đây.
+  //
+  // kPreempted đáng chú ý: mission layer thường đã chuyển sang chặng khác rồi, nên
+  // onNavigationFailed sẽ trả false và không làm gì — đúng ý, chứ không phải bị bỏ sót.
+  const bool accepted = (outcome == NavigationOutcome::kSucceeded)
+                            ? manager->onNavigationDone(mission_sequence_id)
+                            : manager->onNavigationFailed(mission_sequence_id);
+
+  if (!accepted)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    ++stale_outcomes_;
+  }
+}
+
+bool MissionAdapterBridge::hasActiveMission() const
+{
+  mission_adapters::MissionManager* manager = nullptr;
+  bool has_pending = false;
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    manager = manager_;
+    has_pending = static_cast<bool>(pending_);
+  }
+
+  if (has_pending)
+  {
+    return true;
+  }
+  return manager != nullptr && manager->hasMission();
+}
+
+void MissionAdapterBridge::start()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  running_ = true;
+}
+
+void MissionAdapterBridge::stop()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  running_ = false;
+  // Bỏ chặng đang chờ: nó sẽ không bao giờ được chạy, và giữ lại chỉ để nó chạy sau một lần start()
+  // sau này là hành vi không ai mong đợi.
+  pending_.reset();
+}
+
+std::size_t MissionAdapterBridge::droppedRequests() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return dropped_requests_;
+}
+
+std::size_t MissionAdapterBridge::staleOutcomes() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return stale_outcomes_;
+}
+
+}  // namespace move_base2

src/config/move_base2_config.cpp

@@ -0,0 +1,261 @@
+/*********************************************************************
+ * move_base2 — đọc và validate cấu hình runtime.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/config/move_base2_config.h>
+
+#include <cmath>
+#include <sstream>
+
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// Đọc một khoá double; thiếu khoá thì giữ nguyên default và nói rõ khoá nào bị thiếu.
+void readDouble(robot::NodeHandle& nh, const std::string& key, double& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %.4f", key.c_str(), value);
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readInt(robot::NodeHandle& nh, const std::string& key, int& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %d", key.c_str(), value);
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readBool(robot::NodeHandle& nh, const std::string& key, bool& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default %s", key.c_str(),
+                       value ? "true" : "false");
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+void readString(robot::NodeHandle& nh, const std::string& key, std::string& value)
+{
+  if (!nh.hasParam(key))
+  {
+    robot::log_warning("[move_base2] thiếu param '%s', dùng default '%s'", key.c_str(),
+                       value.c_str());
+    return;
+  }
+  nh.param(key, value, value);
+}
+
+/// Đọc một binding profile từ namespace con cùng tên.
+void readBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& binding)
+{
+  robot::NodeHandle profile_nh(nh, ns);
+  readString(profile_nh, "base_global_planner", binding.global_planner_name);
+  readString(profile_nh, "base_local_planner", binding.local_planner_name);
+  readDouble(profile_nh, "xy_goal_tolerance", binding.default_xy_tolerance);
+  readDouble(profile_nh, "yaw_goal_tolerance", binding.default_yaw_tolerance);
+}
+
+bool validateBinding(const ProfileBinding& binding, const char* name, std::string& error)
+{
+  if (binding.local_planner_name.empty())
+  {
+    // Không đặt là hợp lệ: deployment có thể không dùng profile đó. Nhưng nếu đã đặt planner thì
+    // sai số phải hợp lệ, vì chúng đi thẳng vào điều kiện dừng.
+    return true;
+  }
+  if (!std::isfinite(binding.default_xy_tolerance) || binding.default_xy_tolerance <= 0.0)
+  {
+    error = std::string(name) + ".xy_goal_tolerance phải > 0 [m]";
+    return false;
+  }
+  if (!std::isfinite(binding.default_yaw_tolerance) || binding.default_yaw_tolerance <= 0.0)
+  {
+    error = std::string(name) + ".yaw_goal_tolerance phải > 0 [rad]";
+    return false;
+  }
+  return true;
+}
+
+/// Đọc cấu hình đường vào cảm biến từ namespace con `sensors`.
+void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors)
+{
+  robot::NodeHandle sensors_nh(nh, "sensors");
+  readBool(sensors_nh, "laser_sor_enabled", sensors.laser_sor_enabled);
+  readInt(sensors_nh, "laser_sor_mean_k", sensors.laser_sor_mean_k);
+  readDouble(sensors_nh, "laser_sor_stddev_mul", sensors.laser_sor_stddev_mul);
+}
+
+void describeBinding(std::ostringstream& out, const char* name, const ProfileBinding& binding)
+{
+  out << "  " << name << ": global='" << binding.global_planner_name << "' local='"
+      << binding.local_planner_name << "' xy=" << binding.default_xy_tolerance
+      << " m yaw=" << binding.default_yaw_tolerance << " rad\n";
+}
+
+}  // namespace
+
+void MoveBase2Config::fromNodeHandle(robot::NodeHandle& nh)
+{
+  readDouble(nh, "controller_frequency", controller_frequency);
+  readDouble(nh, "planner_frequency", planner_frequency);
+  readDouble(nh, "planner_timeout", planner_timeout);
+
+  readDouble(nh, "planner_patience", state_machine.planner_patience);
+  readDouble(nh, "controller_patience", state_machine.controller_patience);
+  readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
+  readDouble(nh, "oscillation_distance", state_machine.oscillation_distance);
+  readDouble(nh, "action_patience", state_machine.action_patience);
+  readInt(nh, "max_planning_retries", state_machine.max_planning_retries);
+  readBool(nh, "recovery_behavior_enabled", state_machine.recovery_enabled);
+
+  readDouble(nh, "max_vel_x", velocity.max_vel_x);
+  readDouble(nh, "min_vel_x", velocity.min_vel_x);
+  readDouble(nh, "max_vel_theta", velocity.max_vel_theta);
+  readDouble(nh, "acc_lim_x", velocity.max_accel_x);
+  readDouble(nh, "acc_lim_theta", velocity.max_accel_theta);
+
+  readSensors(nh, sensors);
+
+  readBinding(nh, "position", position);
+  readBinding(nh, "docking", docking);
+  readBinding(nh, "go_straight", go_straight);
+  readBinding(nh, "rotate", rotate);
+
+  readString(nh, "recovery_namespace", recovery_namespace);
+  readString(nh, "action_namespace", action_namespace);
+  readString(nh, "mission_namespace", mission_namespace);
+  readString(nh, "global_frame", global_frame);
+  readString(nh, "robot_base_frame", robot_base_frame);
+
+  // recovery_behavior_count KHÔNG đọc từ YAML: nó là số behavior thực sự nạp được, do
+  // RecoveryRunner báo lại sau khi configure. Đọc từ config thì một behavior hỏng sẽ khiến state
+  // machine tin là vẫn còn đường phục hồi.
+}
+
+bool MoveBase2Config::validate(std::string& error) const
+{
+  if (!std::isfinite(controller_frequency) || controller_frequency <= 0.0)
+  {
+    error = "controller_frequency phải > 0 [Hz]";
+    return false;
+  }
+  if (controller_frequency > 200.0)
+  {
+    error = "controller_frequency > 200 Hz — nhịp này không thực tế cho một control loop có costmap";
+    return false;
+  }
+  if (!std::isfinite(planner_frequency) || planner_frequency < 0.0)
+  {
+    error = "planner_frequency phải >= 0 [Hz] (0 = chỉ lập plan khi cần)";
+    return false;
+  }
+  if (!std::isfinite(planner_timeout))
+  {
+    error = "planner_timeout không hữu hạn [s]";
+    return false;
+  }
+
+  if (recovery_namespace.empty())
+  {
+    error = "recovery_namespace rỗng";
+    return false;
+  }
+  if (global_frame.empty() || robot_base_frame.empty())
+  {
+    error = "global_frame và robot_base_frame không được rỗng";
+    return false;
+  }
+  if (global_frame == robot_base_frame)
+  {
+    error = "global_frame trùng robot_base_frame — pose robot sẽ luôn là gốc toạ độ";
+    return false;
+  }
+
+  if (!validateBinding(position, "position", error) ||
+      !validateBinding(docking, "docking", error) ||
+      !validateBinding(go_straight, "go_straight", error) ||
+      !validateBinding(rotate, "rotate", error))
+  {
+    return false;
+  }
+
+  if (position.local_planner_name.empty() && docking.local_planner_name.empty() &&
+      go_straight.local_planner_name.empty() && rotate.local_planner_name.empty())
+  {
+    error = "không profile nào có base_local_planner — runtime sẽ từ chối mọi yêu cầu";
+    return false;
+  }
+
+  if (!velocity.validate(error))
+  {
+    return false;
+  }
+
+  if (!sensors.validate(error))
+  {
+    return false;
+  }
+
+  // Sau cùng: struct con. Thứ tự này có chủ đích — báo lỗi ở tầng cụ thể nhất trước, để thông báo
+  // nói đúng khoá YAML mà người vận hành cần sửa, chứ không phải một ràng buộc phái sinh.
+  //
+  // Lưu ý ràng buộc thứ tự KHỞI TẠO: `state_machine.recovery_behavior_count` KHÔNG đến từ YAML mà
+  // là số behavior RecoveryRunner nạp được thật. Bên gọi phải điền nó trước khi gọi hàm này —
+  // xem @ref MoveBase2Config::validate trong header.
+  if (!state_machine.validate(error))
+  {
+    return false;
+  }
+
+  return true;
+}
+
+std::string MoveBase2Config::describe() const
+{
+  std::ostringstream out;
+  out << "move_base2 config:\n";
+  out << "  controller_frequency: " << controller_frequency << " Hz\n";
+  out << "  planner_frequency: " << planner_frequency << " Hz\n";
+  out << "  planner_timeout: " << planner_timeout << " s\n";
+  out << "  frames: global='" << global_frame << "' base='" << robot_base_frame << "'\n";
+  out << "  namespaces: recovery='" << recovery_namespace << "' actions='" << action_namespace
+      << "' mission='" << mission_namespace << "'\n";
+  describeBinding(out, "position", position);
+  describeBinding(out, "docking", docking);
+  describeBinding(out, "go_straight", go_straight);
+  describeBinding(out, "rotate", rotate);
+  out << state_machine.describe();
+  out << velocity.describe();
+  out << sensors.describe();
+  return out.str();
+}
+
+ControlLoopConfig MoveBase2Config::toControlLoopConfig() const
+{
+  ControlLoopConfig config;
+  config.state_machine = state_machine;
+  config.velocity = velocity;
+  config.nominal_control_period =
+      controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05;  // [s]
+  config.robot_base_frame = robot_base_frame;
+  config.position = position;
+  config.docking = docking;
+  config.go_straight = go_straight;
+  config.rotate = rotate;
+  return config;
+}
+
+}  // namespace move_base2

src/control_loop.cpp

@@ -0,0 +1,569 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt control loop.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/control_loop.h>
+
+#include <cmath>
+#include <sstream>
+
+namespace move_base2
+{
+namespace
+{
+
+/// [-] Sai lệch chuẩn quaternion còn chấp nhận được trước khi coi goal là hỏng.
+constexpr double kQuaternionNormTolerance = 1e-2;
+
+}  // namespace
+
+// ================================================================================================
+// ControlLoopConfig
+// ================================================================================================
+
+bool ControlLoopConfig::validate(std::string& error) const
+{
+  if (!state_machine.validate(error))
+  {
+    return false;
+  }
+  if (!velocity.validate(error))
+  {
+    return false;
+  }
+  if (!(nominal_control_period > 0.0))
+  {
+    error = "nominal_control_period phải > 0 [s]";
+    return false;
+  }
+  if (position.local_planner_name.empty())
+  {
+    error = "profile 'position' bắt buộc phải có local_planner_name";
+    return false;
+  }
+  if (robot_base_frame.empty())
+  {
+    // Frame rỗng đi thẳng vào header của lệnh vận tốc gửi host. Chặn ở đây thay vì để host nhận một
+    // lệnh không biết thuộc hệ toạ độ nào.
+    error = "robot_base_frame không được rỗng";
+    return false;
+  }
+  return true;
+}
+
+std::string ControlLoopConfig::describe() const
+{
+  std::ostringstream out;
+  out << state_machine.describe();
+  out << velocity.describe();
+  out << "ControlLoop:\n";
+  out << "  nominal_control_period: " << nominal_control_period << " s\n";
+  out << "  robot_base_frame      : " << robot_base_frame << '\n';
+  out << "  profile position   : " << position.global_planner_name << " / "
+      << position.local_planner_name << '\n';
+  out << "  profile docking    : " << docking.global_planner_name << " / "
+      << docking.local_planner_name << '\n';
+  out << "  profile go_straight: " << go_straight.global_planner_name << " / "
+      << go_straight.local_planner_name << '\n';
+  out << "  profile rotate     : " << rotate.global_planner_name << " / "
+      << rotate.local_planner_name << '\n';
+  return out.str();
+}
+
+// ================================================================================================
+// ControlLoop
+// ================================================================================================
+
+bool ControlLoop::configure(const ControlLoopConfig& config, const ControlLoopDeps& deps,
+                            std::string& error)
+{
+  initialized_ = false;
+
+  if (deps.clock == nullptr || deps.pose == nullptr || deps.planner == nullptr ||
+      deps.controller == nullptr || deps.recovery == nullptr)
+  {
+    error = "thiếu cổng bắt buộc (clock/pose/planner/controller/recovery)";
+    return false;
+  }
+  if (!config.validate(error))
+  {
+    return false;
+  }
+  if (!state_machine_.configure(config.state_machine, error))
+  {
+    return false;
+  }
+  if (!arbiter_.configure(config.velocity, error))
+  {
+    return false;
+  }
+
+  config_ = config;
+  deps_ = deps;
+  initialized_ = true;
+  reset();
+  return true;
+}
+
+void ControlLoop::reset()
+{
+  state_machine_.reset();
+  arbiter_.reset();
+
+  has_pending_request_ = false;
+  has_active_request_ = false;
+  pause_requested_ = false;
+  resume_requested_ = false;
+  cancel_requested_ = false;
+
+  planner_feedback_ = PlannerFeedback::kIdle;
+  controller_feedback_ = ControllerFeedback::kIdle;
+  recovery_feedback_ = RecoveryFeedback::kIdle;
+  action_feedback_ = ActionFeedback::kIdle;
+
+  latest_plan_.clear();
+  planner_running_ = false;
+
+  // Nhãn mới + huỷ: lượt đang bay thuộc về vòng đời trước, kết quả của nó không được nhận nhầm.
+  ++plan_tag_;
+  if (deps_.planner != nullptr)
+  {
+    deps_.planner->cancelPlan();
+  }
+
+  has_last_cycle_time_ = false;
+  has_oscillation_origin_ = false;
+
+  has_outcome_ = false;
+  outcome_report_count_ = 0;
+  last_reason_ = "";
+}
+
+const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile) const
+{
+  switch (profile)
+  {
+    case MotionProfile::kPosition:
+      return &config_.position;
+    case MotionProfile::kDocking:
+      return &config_.docking;
+    case MotionProfile::kGoStraight:
+      return &config_.go_straight;
+    case MotionProfile::kRotate:
+      return &config_.rotate;
+  }
+  return nullptr;
+}
+
+bool ControlLoop::isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose)
+{
+  const auto& q = pose.pose.orientation;
+  if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
+  {
+    return false;
+  }
+  const double norm_sq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+  return std::abs(std::sqrt(norm_sq) - 1.0) <= kQuaternionNormTolerance;
+}
+
+bool ControlLoop::submit(const NavigationRequest& request, std::string& reason)
+{
+  if (!initialized_)
+  {
+    reason = "runtime chưa khởi tạo";
+    return false;
+  }
+
+  // D8: yêu cầu mang action cần có ActionPort; từ chối tại cửa thay vì kẹt sau khi tới goal.
+  if (!request.actions.empty() && deps_.action == nullptr)
+  {
+    reason = "yêu cầu có action nhưng runtime không có action port";
+    return false;
+  }
+
+  if (!request.has_goal)
+  {
+    // D8: yêu cầu chỉ-có-action — không có goal để validate, không có planner để swap.
+    if (request.actions.empty())
+    {
+      reason = "yêu cầu không có goal lẫn action";
+      return false;
+    }
+    pending_request_ = request;
+    has_pending_request_ = true;
+    cancel_requested_ = false;
+    return true;
+  }
+
+  if (!std::isfinite(request.goal.pose.position.x) || !std::isfinite(request.goal.pose.position.y))
+  {
+    reason = "goal có toạ độ không hữu hạn";
+    return false;
+  }
+  if (!isQuaternionValid(request.goal))
+  {
+    reason = "goal có quaternion không hợp lệ";
+    return false;
+  }
+
+  const ProfileBinding* binding = bindingFor(request.profile);
+  if (binding == nullptr || binding->local_planner_name.empty())
+  {
+    reason = std::string("chưa cấu hình planner cho profile '") + toString(request.profile) + "'";
+    return false;
+  }
+
+  // Đổi planner NGAY tại cửa vào, trước khi nhận yêu cầu: nếu không nạp được thì từ chối luôn, chứ
+  // không để state machine bắt đầu một chặng rồi mới phát hiện không có planner nào chạy được.
+  if (!binding->global_planner_name.empty() &&
+      !deps_.planner->swapPlanner(binding->global_planner_name))
+  {
+    reason = "không nạp được global planner '" + binding->global_planner_name + "'";
+    return false;
+  }
+  if (!deps_.controller->swapPlanner(binding->local_planner_name))
+  {
+    reason = "không nạp được local planner '" + binding->local_planner_name + "'";
+    return false;
+  }
+
+  deps_.controller->setTolerance(
+      request.tolerance.hasXy() ? request.tolerance.xy : binding->default_xy_tolerance,
+      request.tolerance.hasYaw() ? request.tolerance.yaw : binding->default_yaw_tolerance);
+
+  pending_request_ = request;
+  has_pending_request_ = true;
+
+  // Yêu cầu mới thay thế yêu cầu đang chờ, không xếp hàng: xếp hàng là việc của mission layer.
+  cancel_requested_ = false;
+  return true;
+}
+
+void ControlLoop::requestPause()
+{
+  pause_requested_ = true;
+  resume_requested_ = false;
+}
+
+void ControlLoop::requestResume()
+{
+  resume_requested_ = true;
+  pause_requested_ = false;
+}
+
+void ControlLoop::requestCancel()
+{
+  cancel_requested_ = true;
+}
+
+void ControlLoop::collectPlannerResult()
+{
+  PlanResult result;
+  if (!deps_.planner->pollPlan(result))
+  {
+    // Chưa có kết quả. Chỉ báo "đang lập" khi thật sự có lượt đang chạy VÀ chưa có phản hồi nào
+    // khác — `apply_plan` ở cycle trước có thể đã đặt kFailed khi controller từ chối plan, và đó
+    // là tin quan trọng hơn.
+    if (planner_running_ && planner_feedback_ == PlannerFeedback::kIdle)
+    {
+      planner_feedback_ = PlannerFeedback::kBusy;
+    }
+    return;
+  }
+
+  planner_running_ = false;
+
+  if (result.tag != plan_tag_)
+  {
+    // Kết quả của một yêu cầu đã bị thay hoặc huỷ. Vứt lặng lẽ: bám theo nó nghĩa là robot đi tới
+    // goal không còn ai yêu cầu. Không phải lỗi, nên cũng không đặt kFailed.
+    return;
+  }
+
+  // Contract của PlannerPort là "succeeded nghĩa là plan không rỗng". Vẫn kiểm lại ở đây vì plan
+  // rỗng lọt xuống sẽ thành front()/back() trên vector rỗng ở tầng dưới.
+  if (!result.succeeded || result.plan.empty())
+  {
+    planner_feedback_ = PlannerFeedback::kFailed;
+    return;
+  }
+
+  latest_plan_.swap(result.plan);
+  planner_feedback_ = PlannerFeedback::kPlanReady;
+}
+
+void ControlLoop::runController(robot_geometry_msgs::Twist& candidate)
+{
+  candidate = robot_geometry_msgs::Twist();
+
+  // Giữ nguyên thứ tự của interface được bọc: hỏi đã tới đích trước, chỉ khi chưa mới tính lệnh.
+  if (deps_.controller->isGoalReached())
+  {
+    controller_feedback_ = ControllerFeedback::kGoalReached;
+    return;
+  }
+
+  robot_geometry_msgs::Twist cmd;
+  if (deps_.controller->computeVelocityCommands(cmd))
+  {
+    controller_feedback_ = ControllerFeedback::kCommandValid;
+    candidate = cmd;
+    return;
+  }
+
+  controller_feedback_ = ControllerFeedback::kNoValidCommand;
+}
+
+bool ControlLoop::step()
+{
+  if (!initialized_)
+  {
+    return false;
+  }
+
+  // --- 1. Thời gian và pose -------------------------------------------------------------------
+  const robot::Time now = deps_.clock->now();
+
+  double dt = config_.nominal_control_period;
+  if (has_last_cycle_time_)
+  {
+    dt = (now - last_cycle_time_).toSec();
+    if (dt < 0.0)
+    {
+      // Đồng hồ lùi (thường do đổi nguồn time). Không có dt tin được thì bỏ giới hạn gia tốc ở
+      // cycle này thay vì tính ra một giá trị bịa.
+      dt = 0.0;
+    }
+  }
+  last_cycle_time_ = now;
+  has_last_cycle_time_ = true;
+
+  // Thu kết quả lập plan TRƯỚC khi dựng dữ liệu vào: state machine quyết định dựa trên phản hồi,
+  // nên phản hồi phải có mặt trước lúc nó chạy.
+  collectPlannerResult();
+
+  robot_geometry_msgs::PoseStamped robot_pose;
+  const bool pose_available = deps_.pose->getRobotPose(robot_pose);
+
+  double travelled = 0.0;
+  if (pose_available && has_oscillation_origin_)
+  {
+    const double dx = robot_pose.pose.position.x - oscillation_origin_.pose.position.x;
+    const double dy = robot_pose.pose.position.y - oscillation_origin_.pose.position.y;
+    travelled = std::hypot(dx, dy);
+  }
+
+  // --- 2. State machine -----------------------------------------------------------------------
+  StateMachineInput input;
+  input.now = now;
+  input.has_pending_request = has_pending_request_;
+  input.pending_request_has_goal = pending_request_.has_goal;
+  input.pending_request_action_count = pending_request_.actions.size();
+  input.pause_requested = pause_requested_;
+  input.resume_requested = resume_requested_;
+  input.cancel_requested = cancel_requested_;
+  input.planner = planner_feedback_;
+  input.controller = controller_feedback_;
+  input.recovery = recovery_feedback_;
+  input.action = action_feedback_;
+  input.pose_available = pose_available;
+  input.robot_stopped = arbiter_.stopped();
+  input.travelled_since_oscillation_reset = travelled;
+
+  // Cùng một chỉ số dùng cho cả cycle khởi động recovery lẫn các cycle tick: state machine chỉ tăng
+  // chỉ số khi behavior kết thúc, nên `nextRecoveryIndex()` chính là behavior sắp chạy hoặc đang
+  // chạy. Hỏi trước khi state machine quyết định để nó biết có nên trao quyền phát vận tốc không.
+  input.active_recovery_output =
+      deps_.recovery != nullptr
+          ? deps_.recovery->outputKind(state_machine_.nextRecoveryIndex())
+          : RecoveryOutputKind::kNone;
+
+  const StateMachineOutput output = state_machine_.update(input);
+
+  if (output.state_changed)
+  {
+    last_reason_ = output.reason;
+  }
+
+  // Cờ một-lần đã được state machine tiêu thụ xong ở lời gọi trên.
+  pause_requested_ = false;
+  resume_requested_ = false;
+
+  // Phản hồi của cycle trước đã dùng xong; đặt lại để cycle này tự sinh phản hồi mới.
+  planner_feedback_ = PlannerFeedback::kIdle;
+  controller_feedback_ = ControllerFeedback::kIdle;
+  recovery_feedback_ = RecoveryFeedback::kIdle;
+  action_feedback_ = ActionFeedback::kIdle;
+
+  // --- 3. Thi hành output ---------------------------------------------------------------------
+  if (output.accept_request)
+  {
+    active_request_ = pending_request_;
+    has_active_request_ = true;
+    has_pending_request_ = false;
+    latest_plan_.clear();
+    has_outcome_ = false;
+
+    // Nhãn mới: mọi lượt lập plan đang bay thuộc về goal cũ và phải bị vứt khi về.
+    ++plan_tag_;
+    deps_.planner->cancelPlan();
+    planner_running_ = false;
+  }
+
+  if (output.reset_oscillation_origin && pose_available)
+  {
+    oscillation_origin_ = robot_pose;
+    has_oscillation_origin_ = true;
+  }
+
+  if (output.stop_planner)
+  {
+    // Huỷ thật, không chỉ quên đi: nếu chỉ hạ cờ thì lượt đang chạy vẫn về và chiếm chỗ hộp thư,
+    // rồi bị nhận nhầm cho lượt kế tiếp mang cùng nhãn.
+    deps_.planner->cancelPlan();
+    planner_running_ = false;
+  }
+
+  if (output.apply_plan && !latest_plan_.empty())
+  {
+    if (!deps_.controller->setPlan(latest_plan_))
+    {
+      // Controller từ chối plan: coi như lần lập plan này hỏng, để chu kỳ kiên nhẫn tiếp tục chạy.
+      planner_feedback_ = PlannerFeedback::kFailed;
+    }
+  }
+
+  if (output.cancel_recovery)
+  {
+    deps_.recovery->cancel();
+  }
+
+  robot_geometry_msgs::Twist candidate;
+
+  if (output.start_recovery)
+  {
+    if (!deps_.recovery->start(output.recovery_index, output.recovery_trigger))
+    {
+      // Behavior từ chối khởi động (ví dụ đã va chạm ngay tại chỗ) — báo hỏng để state machine
+      // chuyển sang behavior kế tiếp thay vì tick một behavior chưa start.
+      recovery_feedback_ = RecoveryFeedback::kFailed;
+    }
+  }
+  else if (output.tick_recovery)
+  {
+    const RecoveryTick tick = deps_.recovery->update();
+    switch (tick.status)
+    {
+      case RecoveryTick::Status::kRunning:
+        recovery_feedback_ = RecoveryFeedback::kRunning;
+        break;
+      case RecoveryTick::Status::kSucceeded:
+        recovery_feedback_ = RecoveryFeedback::kSucceeded;
+        break;
+      case RecoveryTick::Status::kFailed:
+        recovery_feedback_ = RecoveryFeedback::kFailed;
+        break;
+    }
+    if (tick.has_velocity)
+    {
+      candidate = tick.cmd;
+    }
+    if (tick.has_path && !tick.path.empty())
+    {
+      // Họ recovery sinh lại đường đi: coi kết quả như một plan mới, chờ state machine đẩy xuống.
+      latest_plan_ = tick.path;
+      planner_feedback_ = PlannerFeedback::kPlanReady;
+    }
+  }
+
+  if (output.cancel_action && deps_.action != nullptr)
+  {
+    deps_.action->cancel();
+  }
+
+  if (output.start_action)
+  {
+    // Cửa submit đã chặn yêu cầu có action mà không có port, nhưng vẫn guard: kẹt ở đây nghĩa là
+    // contract bị phá từ một đường khác — báo action hỏng để state machine kết thúc tường minh.
+    if (deps_.action == nullptr || output.action_index >= active_request_.actions.size())
+    {
+      action_feedback_ = ActionFeedback::kFailed;
+    }
+    else if (!deps_.action->start(active_request_.actions[output.action_index]))
+    {
+      // Không có handler cho actionType này hoặc handler từ chối — action coi như thất bại.
+      action_feedback_ = ActionFeedback::kFailed;
+    }
+  }
+  else if (output.tick_action && deps_.action != nullptr)
+  {
+    const ActionTick tick = deps_.action->update();
+    switch (tick.status)
+    {
+      case ActionTick::Status::kRunning:
+        action_feedback_ = ActionFeedback::kRunning;
+        break;
+      case ActionTick::Status::kSucceeded:
+        action_feedback_ = ActionFeedback::kSucceeded;
+        break;
+      case ActionTick::Status::kFailed:
+        action_feedback_ = ActionFeedback::kFailed;
+        break;
+    }
+  }
+
+  if (output.run_controller)
+  {
+    runController(candidate);
+  }
+
+  if (output.start_planner && pose_available && !planner_running_)
+  {
+    // `start_planner` là tín hiệu MỨC ("hãy đang lập plan"), bật lại mỗi cycle chừng nào state
+    // machine còn ở kPlanning — không phải sườn. Kick lại một lượt đang chạy sẽ vừa bị cổng từ
+    // chối, vừa làm mất thời gian đã bỏ ra.
+    planner_running_ = deps_.planner->startPlan(robot_pose, active_request_.goal,
+                                                active_request_.order.get(), plan_tag_);
+    if (!planner_running_)
+    {
+      // Không khởi động được (chưa có planner, pose hỏng...). Coi như một lượt hỏng để đồng hồ
+      // kiên nhẫn tiếp tục chạy, thay vì đứng im ở kPlanning vô hạn.
+      planner_feedback_ = PlannerFeedback::kFailed;
+    }
+  }
+
+  // --- 4. Lệnh vận tốc ------------------------------------------------------------------------
+  arbiter_.arbitrate(output.velocity_source, candidate, dt);
+
+  // --- 5. Báo kết quả -------------------------------------------------------------------------
+  if (output.report_outcome)
+  {
+    last_outcome_ = output.outcome;
+    has_outcome_ = true;
+    ++outcome_report_count_;
+
+    if (deps_.mission != nullptr && has_active_request_ &&
+        active_request_.mission_sequence_id != 0)
+    {
+      deps_.mission->reportOutcome(active_request_.mission_sequence_id, output.outcome);
+    }
+
+    has_active_request_ = false;
+    cancel_requested_ = false;
+    latest_plan_.clear();
+    planner_running_ = false;
+  }
+
+  return !output.report_outcome;
+}
+
+const char* ControlLoop::lastOutcome() const
+{
+  return has_outcome_ ? toString(last_outcome_) : "";
+}
+
+}  // namespace move_base2

src/io/sensor_gateway.cpp

@@ -0,0 +1,337 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt cửa vào dữ liệu cảm biến.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/io/sensor_gateway.h>
+
+#include <exception>
+#include <sstream>
+
+#include <laser_filter/laser_filter.h>
+#include <robot/console.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// [s] Giãn cách log cho các cảnh báo phát sinh trong đường nóng — tần số cảm biến, không được spam.
+constexpr double kHotPathLogThrottle = 5.0;
+
+const char* toString(robot_costmap_2d::LayerType type)
+{
+  switch (type)
+  {
+    case robot_costmap_2d::LayerType::STATIC_LAYER:
+      return "StaticLayer";
+    case robot_costmap_2d::LayerType::OBSTACLE_LAYER:
+      return "ObstacleLayer";
+    case robot_costmap_2d::LayerType::VOXEL_LAYER:
+      return "VoxelLayer";
+    case robot_costmap_2d::LayerType::INFLATION_LAYER:
+      return "InflationLayer";
+    case robot_costmap_2d::LayerType::CRITICAL_LAYER:
+      return "CriticalLayer";
+    case robot_costmap_2d::LayerType::DIRECTIONAL_LAYER:
+      return "DirectionalLayer";
+    case robot_costmap_2d::LayerType::PREFERRED_LAYER:
+      return "PreferredLayer";
+    case robot_costmap_2d::LayerType::UNPREFERRED_LAYER:
+      return "UnpreferredLayer";
+    case robot_costmap_2d::LayerType::UNKNOWN:
+      break;
+  }
+  return "Unknown";
+}
+
+}  // namespace
+
+// ================================================================================================
+// SensorGatewayConfig
+// ================================================================================================
+
+bool SensorGatewayConfig::validate(std::string& error) const
+{
+  // Chỉ kiểm khi lọc bật: tham số của một tính năng đang tắt không nên chặn được cả runtime.
+  if (!laser_sor_enabled)
+  {
+    return true;
+  }
+
+  if (laser_sor_mean_k < 2)
+  {
+    error = "laser_sor_mean_k phải >= 2 [điểm] khi laser_sor_enabled = true";
+    return false;
+  }
+  if (!(laser_sor_stddev_mul > 0.0))
+  {
+    error = "laser_sor_stddev_mul phải > 0 khi laser_sor_enabled = true";
+    return false;
+  }
+  return true;
+}
+
+std::string SensorGatewayConfig::describe() const
+{
+  std::ostringstream out;
+  out << "SensorGateway:\n";
+  out << "  laser_sor_enabled     : " << (laser_sor_enabled ? "true" : "false") << '\n';
+  if (laser_sor_enabled)
+  {
+    out << "  laser_sor_mean_k      : " << laser_sor_mean_k << " điểm\n";
+    out << "  laser_sor_stddev_mul  : " << laser_sor_stddev_mul << '\n';
+  }
+  return out.str();
+}
+
+// ================================================================================================
+// SensorGateway
+// ================================================================================================
+
+SensorGateway::SensorGateway() = default;
+
+// Định nghĩa ở đây (không phải `= default` trong header) vì laser_sor_ là unique_ptr tới kiểu chưa
+// hoàn chỉnh ở phía header.
+SensorGateway::~SensorGateway() = default;
+
+bool SensorGateway::configure(const SensorGatewayConfig& config, std::string& error)
+{
+  if (!config.validate(error))
+  {
+    return false;
+  }
+
+  config_ = config;
+
+  if (config_.laser_sor_enabled)
+  {
+    // Dựng một lần, không phải mỗi mẫu như bản cũ. Tham số cũng chỉ set ở đây.
+    laser_sor_.reset(new laser_filter::LaserScanSOR());
+    laser_sor_->setMeanK(config_.laser_sor_mean_k);
+    laser_sor_->setStddevMulThresh(config_.laser_sor_stddev_mul);
+  }
+  else
+  {
+    laser_sor_.reset();
+  }
+
+  configured_ = true;
+  return true;
+}
+
+void SensorGateway::attach(robot_costmap_2d::LayeredCostmap* global,
+                           robot_costmap_2d::LayeredCostmap* local)
+{
+  global_costmap_ = global;
+  local_costmap_ = local;
+
+  warnAboutUnreachableLayers(global_costmap_, "global");
+  warnAboutUnreachableLayers(local_costmap_, "local");
+}
+
+bool SensorGateway::attached() const
+{
+  return global_costmap_ != nullptr || local_costmap_ != nullptr;
+}
+
+void SensorGateway::warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap* costmap,
+                                               const char* which) const
+{
+  if (costmap == nullptr)
+  {
+    return;
+  }
+
+  auto* plugins = costmap->getPlugins();
+  if (plugins == nullptr)
+  {
+    return;
+  }
+
+  // Dữ liệu vật cản được đẩy theo LayerType::VOXEL_LAYER, đúng như bản cũ. Một layer khai
+  // `type: ObstacleLayer` thuần sẽ không bao giờ khớp và không nhận được gì — im lặng. Đây là bẫy
+  // có thật: cây config `robot_costmap_2d/config/costmap_params.yaml` đang khai đúng kiểu đó.
+  // Không tự ý mở rộng đích để tránh đổi hành vi; thay vào đó nói ra lúc khởi tạo.
+  for (const auto& layer : *plugins)
+  {
+    if (!layer)
+    {
+      continue;
+    }
+    if (layer->getType() == robot_costmap_2d::LayerType::OBSTACLE_LAYER)
+    {
+      robot::log_warning(
+          "[SensorGateway] costmap %s: layer '%s' kiểu ObstacleLayer sẽ KHÔNG nhận dữ liệu cảm "
+          "biến — cổng này đẩy vật cản theo LayerType::VOXEL_LAYER. Đổi sang 'type: VoxelLayer' "
+          "trong danh sách plugins nếu layer đó cần dữ liệu.\n",
+          which, layer->getName().c_str());
+    }
+  }
+}
+
+robot_sensor_msgs::LaserScan SensorGateway::prepareLaserScan(
+    const robot_sensor_msgs::LaserScan& scan) const
+{
+  if (!config_.laser_sor_enabled || laser_sor_ == nullptr)
+  {
+    return scan;
+  }
+  return laser_sor_->filter(scan);
+}
+
+namespace
+{
+
+/**
+ * @brief Đưa một mẫu tới mọi layer khớp kiểu trong một costmap.
+ *
+ * Template nằm trong .cpp có chủ đích: nó là chỗ duy nhất chạm `dataCallBack`, và giữ nó ở đây làm
+ * cho `sensor_gateway.h` không phải kéo theo `robot_costmap_2d`.
+ */
+template <typename T>
+void dispatchTo(robot_costmap_2d::LayeredCostmap* costmap, const T& value,
+                robot_costmap_2d::LayerType type, const std::string& name,
+                SensorGatewayStats& stats)
+{
+  if (costmap == nullptr)
+  {
+    return;
+  }
+
+  auto* plugins = costmap->getPlugins();
+  if (plugins == nullptr)
+  {
+    return;
+  }
+
+  for (const auto& layer : *plugins)
+  {
+    if (!layer)
+    {
+      continue;
+    }
+
+    // Lọc CHỈ theo kiểu. Vế `|| getName() == name` của bản cũ bị bỏ — xem doc của lớp.
+    if (layer->getType() != type)
+    {
+      continue;
+    }
+
+    if (!layer->isEnabled())
+    {
+      ++stats.skipped_disabled;
+      continue;
+    }
+
+    try
+    {
+      layer->dataCallBack<T>(value, name);
+      ++stats.delivered;
+    }
+    catch (const std::exception& ex)
+    {
+      // Bắt quanh TỪNG layer: bản cũ bắt quanh cả vòng lặp rồi return, nên một layer hỏng làm mọi
+      // layer đứng sau nó mất luôn mẫu này.
+      ++stats.layer_exceptions;
+      robot::log_error_throttle(
+          kHotPathLogThrottle,
+          "[SensorGateway] layer '%s' (%s) ném exception khi nhận '%s': %s\n",
+          layer->getName().c_str(), toString(type), name.c_str(), ex.what());
+    }
+  }
+}
+
+}  // namespace
+
+void SensorGateway::pushStaticMap(const std::string& name, const robot_nav_msgs::OccupancyGrid& map)
+{
+  if (!attached())
+  {
+    ++stats_.dropped_no_costmap;
+    robot::log_warning_throttle(kHotPathLogThrottle,
+                                "[SensorGateway] bỏ static map '%s': chưa gắn costmap nào\n",
+                                name.c_str());
+    return;
+  }
+
+  dispatchTo(global_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
+  dispatchTo(local_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
+}
+
+void SensorGateway::pushLaserScan(const std::string& name, const robot_sensor_msgs::LaserScan& scan)
+{
+  if (!attached())
+  {
+    ++stats_.dropped_no_costmap;
+    robot::log_warning_throttle(kHotPathLogThrottle,
+                                "[SensorGateway] bỏ laser scan '%s': chưa gắn costmap nào\n",
+                                name.c_str());
+    return;
+  }
+
+  dispatchTo(local_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+  dispatchTo(global_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+}
+
+void SensorGateway::pushPointCloud(const std::string& name,
+                                   const robot_sensor_msgs::PointCloud& cloud)
+{
+  if (!attached())
+  {
+    ++stats_.dropped_no_costmap;
+    robot::log_warning_throttle(kHotPathLogThrottle,
+                                "[SensorGateway] bỏ point cloud '%s': chưa gắn costmap nào\n",
+                                name.c_str());
+    return;
+  }
+
+  dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+  dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+}
+
+void SensorGateway::pushPointCloud2(const std::string& name,
+                                    const robot_sensor_msgs::PointCloud2& cloud)
+{
+  if (!attached())
+  {
+    ++stats_.dropped_no_costmap;
+    robot::log_warning_throttle(kHotPathLogThrottle,
+                                "[SensorGateway] bỏ point cloud2 '%s': chưa gắn costmap nào\n",
+                                name.c_str());
+    return;
+  }
+
+  dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+  dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
+}
+
+void SensorGateway::pushDepthCameraData(const std::string& topic,
+                                        const robot_sensor_msgs::DepthCameraData::ConstPtr& data)
+{
+  if (data == nullptr)
+  {
+    return;
+  }
+
+  if (!attached())
+  {
+    ++stats_.dropped_no_costmap;
+    robot::log_warning_throttle(kHotPathLogThrottle,
+                                "[SensorGateway] bỏ depth camera '%s': chưa gắn costmap nào\n",
+                                topic.c_str());
+    return;
+  }
+
+  // Phải giữ nguyên dạng ConstPtr: layer so `typeid(DepthCameraData::ConstPtr)`. Truyền giá trị sẽ
+  // rơi im lặng qua mọi nhánh của handleImpl.
+  dispatchTo(local_costmap_, data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic, stats_);
+  dispatchTo(global_costmap_, data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic, stats_);
+}
+
+}  // namespace move_base2

src/move_base2_plugin.cpp

@@ -0,0 +1,35 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — export plugin Boost.DLL.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <memory>
+
+#include <boost/dll/alias.hpp>
+
+#include <move_base_core/navigation.h>
+
+#include <move_base2/navigation_server.h>
+
+namespace move_base2
+{
+
+/**
+ * @brief Factory được host nạp qua boost::dll::import_alias.
+ *
+ * Kiểu trả về phải khớp CHÍNH XÁC kiểu mà loader khai báo:
+ * `robot::move_base_core::BaseNavigation::Ptr()`. Cơ chế nạp là dlsym + reinterpret_cast, không có
+ * kiểm kiểu nào qua ranh giới .so — lệch kiểu ở đây không gây lỗi biên dịch mà gây hỏng bộ nhớ lúc
+ * chạy. Đổi contract thì phải đổi cả loader trong cùng một lần sửa.
+ */
+robot::move_base_core::BaseNavigation::Ptr createMoveBase2()
+{
+  return std::make_shared<NavigationServer>();
+}
+
+}  // namespace move_base2
+
+BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase2)

src/navigation_server.cpp

@@ -0,0 +1,653 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt facade BaseNavigation.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/navigation_server.h>
+
+#include <cmath>
+
+#include <robot_nav_2d_utils/conversions.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// @brief Lấy phần tử theo khoá, trả về giá trị mặc định nếu không có.
+template <typename MapT>
+typename MapT::mapped_type lookupOrDefault(const MapT& container,
+                                           const typename MapT::key_type& key)
+{
+  const auto it = container.find(key);
+  return it == container.end() ? typename MapT::mapped_type() : it->second;
+}
+
+}  // namespace
+
+NavigationServer::NavigationServer()
+{
+  // nav_feedback_ là thành viên của contract host và được host đọc qua con trỏ, nên phải tồn tại
+  // ngay từ lúc dựng — trước cả initialize().
+  nav_feedback_ = std::make_shared<robot::move_base_core::NavFeedback>();
+  nav_feedback_->navigation_state = robot::move_base_core::State::PENDING;
+  nav_feedback_->feed_back_str = "chưa khởi tạo";
+  nav_feedback_->goal_checked = false;
+  nav_feedback_->is_ready = false;
+}
+
+NavigationServer::~NavigationServer() = default;
+
+// ================================================================================================
+// Cấu hình lõi
+// ================================================================================================
+
+bool NavigationServer::configureLoop(const ControlLoopConfig& config, const ControlLoopDeps& deps,
+                                     std::string& error)
+{
+  if (!loop_.configure(config, deps, error))
+  {
+    nav_feedback_->is_ready = false;
+    nav_feedback_->feed_back_str = "cấu hình lỗi: " + error;
+    return false;
+  }
+
+  robot_base_frame_ = config.robot_base_frame;
+
+  nav_feedback_->is_ready = true;
+  nav_feedback_->feed_back_str = "sẵn sàng";
+  refreshFeedback();
+  return true;
+}
+
+bool NavigationServer::configureSensors(const SensorGatewayConfig& config, std::string& error)
+{
+  return sensors_.configure(config, error);
+}
+
+void NavigationServer::attachCostmaps(robot_costmap_2d::LayeredCostmap* global,
+                                      robot_costmap_2d::LayeredCostmap* local)
+{
+  sensors_.attach(global, local);
+
+  // Phát lại static map đã nhận trước khi costmap tồn tại. Chỉ static map, cố ý: một laser scan cũ
+  // phát lại vào costmap là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu — im lặng và nguy
+  // hiểm hơn hẳn việc chờ mẫu kế tiếp, vốn chỉ cách vài chục ms.
+  std::map<std::string, robot_nav_msgs::OccupancyGrid> maps;
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    maps = static_maps_;
+
+    // `map_save_`/`map_name_save_` là cặp biến PUBLIC của BaseNavigation mà host tự gán (đường bù
+    // của bản cũ cho đúng vấn đề này). Tôn trọng nó để host không phải sửa gì, nhưng không để nó
+    // ghi đè bản đã đi qua addStaticMap.
+    if (!map_name_save_.empty() && maps.find(map_name_save_) == maps.end())
+    {
+      maps[map_name_save_] = map_save_;
+    }
+  }
+
+  for (const auto& entry : maps)
+  {
+    sensors_.pushStaticMap(entry.first, entry.second);
+  }
+}
+
+bool NavigationServer::spinOnce()
+{
+  // Trước khi tính lệnh: đẩy xuống controller những gì host đã đặt từ thread của nó. Đặt ở đây chứ
+  // không ở cuối cycle để trần vận tốc có hiệu lực ngay trong chính cycle này — chậm một cycle
+  // nghĩa là một chu kỳ nữa chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ.
+  pushHostInputsToController();
+
+  const bool running = loop_.step();
+  publishCommand();
+  refreshFeedback();
+  return running;
+}
+
+void NavigationServer::publishCommand()
+{
+  // getTwist() của contract host là LỆNH vận tốc đang phát, không phải vận tốc đo được: host lấy nó
+  // rồi publish thẳng ra cmd_vel (amr_publiser.cpp:360-370). Đổ odometry vào đây tạo vòng lặp dương
+  // — robot giữ nguyên tốc độ hiện tại vô hạn và VelocityArbiter bị vô hiệu hoàn toàn.
+  //
+  // Nguồn duy nhất đúng là lệnh vừa qua bộ trọng tài. Dấu thời gian lấy theo cycle của control loop
+  // chứ không phải giờ hệ thống lúc gọi: host loại lệnh quá hạn, nên control loop treo phải làm dấu
+  // thời gian đứng yên để host thấy được và ngừng phát.
+  const robot_geometry_msgs::Twist& command = loop_.lastCommand();
+
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  twist_.velocity = robot_nav_2d_utils::twist3Dto2D(command);
+  twist_.header.stamp = loop_.lastCycleTime();
+  twist_.header.frame_id = robot_base_frame_;
+}
+
+robot::move_base_core::State NavigationServer::toHostState(NavigationState state)
+{
+  using HostState = robot::move_base_core::State;
+  switch (state)
+  {
+    case NavigationState::kIdle:
+      return HostState::PENDING;
+    case NavigationState::kPlanning:
+      return HostState::PLANNING;
+    case NavigationState::kControlling:
+      return HostState::CONTROLLING;
+    case NavigationState::kRecovering:
+      // Contract host chỉ có CLEARING cho giai đoạn phục hồi. Ánh xạ về đó để host cũ không phải
+      // đổi gì; ngữ nghĩa mới (recovery có thời lượng, có thể phát vận tốc) nằm ở phía lõi.
+      return HostState::CLEARING;
+    case NavigationState::kExecutingActions:
+      // Contract host không có khái niệm action (D8 mới thêm). ACTIVE (actionlib: goal đang được
+      // xử lý) là ánh xạ đúng: giữ nghĩa "chặng chưa xong" nhưng KHÔNG phải CONTROLLING — host
+      // VDA5050 đang suy `driving = true` từ CONTROLLING (amr_vda_5050_client_api.cpp:1233), mà
+      // robot lúc này đứng yên làm action; báo "đang chạy" cho fleet master là báo sai.
+      return HostState::ACTIVE;
+    case NavigationState::kPaused:
+      return HostState::PAUSED;
+    case NavigationState::kCancelling:
+      return HostState::PREEMPTING;
+    case NavigationState::kSucceeded:
+      return HostState::SUCCEEDED;
+    case NavigationState::kAborted:
+      return HostState::ABORTED;
+    case NavigationState::kCancelled:
+      return HostState::PREEMPTED;
+  }
+  return HostState::LOST;
+}
+
+void NavigationServer::refreshFeedback()
+{
+  nav_feedback_->navigation_state = toHostState(loop_.state());
+
+  const char* reason = loop_.lastReason();
+  if (reason != nullptr && reason[0] != '\0')
+  {
+    nav_feedback_->feed_back_str = reason;
+  }
+
+  robot_geometry_msgs::Pose2D pose2d;
+  nav_feedback_->goal_checked = getRobotPose(pose2d);
+  if (nav_feedback_->goal_checked)
+  {
+    nav_feedback_->current_pose = pose2d;
+  }
+}
+
+// ================================================================================================
+// Khởi tạo
+// ================================================================================================
+
+void NavigationServer::initialize(robot::TFListenerPtr tf)
+{
+  tf_ = tf;
+
+  // Phần dựng costmap, planner runner, controller runner và recovery runner từ tf này thuộc bước
+  // nối dây runtime. Cho tới lúc đó, các cổng phải được bơm vào qua configureLoop() — cố ý KHÔNG
+  // tự dựng cổng giả ở đây, vì một runtime chạy được với cổng giả là thứ nguy hiểm nhất có thể có.
+  if (!loop_.initialized())
+  {
+    nav_feedback_->is_ready = false;
+    nav_feedback_->feed_back_str = "đã nhận tf, chờ configureLoop() nạp các cổng runtime";
+  }
+}
+
+// ================================================================================================
+// Footprint
+// ================================================================================================
+
+void NavigationServer::setRobotFootprint(const std::vector<robot_geometry_msgs::Point>& fprt)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  footprint_ = fprt;
+}
+
+std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return footprint_;
+}
+
+// ================================================================================================
+// Nhận dữ liệu sensor
+// ================================================================================================
+
+// Khuôn chung của cả năm hàm dưới đây: cất giữ dưới `data_mutex_`, ĐÓNG lock, rồi mới đẩy vào
+// costmap. Thứ tự đó là bắt buộc chứ không phải phong cách — `StaticLayer::incomingMap` có thể gọi
+// `LayeredCostmap::resizeMap`, hàm này chờ mutex master costmap và có thể đứng trọn một chu kỳ
+// `updateMap`. Đẩy trong lúc còn giữ `data_mutex_` sẽ kéo theo `getTwist`/`getRobotFootprint`/
+// `getStaticMap` của host chết chờ cùng, trong khi host chỉ có MỘT thread phục vụ mọi callback.
+
+void NavigationServer::addStaticMap(const std::string& map_name, robot_nav_msgs::OccupancyGrid map)
+{
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    static_maps_[map_name] = map;
+  }
+  sensors_.pushStaticMap(map_name, map);
+}
+
+void NavigationServer::addLaserScan(const std::string& laser_scan_name,
+                                    robot_sensor_msgs::LaserScan laser_scan)
+{
+  // Lọc TRƯỚC khi cất: bản được cất và bản costmap nhìn thấy phải là một. Bản cũ cũng cất bản đã
+  // lọc; nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật sẽ lệch nhau.
+  const robot_sensor_msgs::LaserScan prepared = sensors_.prepareLaserScan(laser_scan);
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    laser_scans_[laser_scan_name] = prepared;
+  }
+  sensors_.pushLaserScan(laser_scan_name, prepared);
+}
+
+void NavigationServer::addPointCloud(const std::string& point_cloud_name,
+                                     robot_sensor_msgs::PointCloud point_cloud)
+{
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    point_clouds_[point_cloud_name] = point_cloud;
+  }
+  sensors_.pushPointCloud(point_cloud_name, point_cloud);
+}
+
+void NavigationServer::addPointCloud2(const std::string& point_cloud2_name,
+                                      robot_sensor_msgs::PointCloud2 point_cloud2)
+{
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    point_cloud2s_[point_cloud2_name] = point_cloud2;
+  }
+  sensors_.pushPointCloud2(point_cloud2_name, point_cloud2);
+}
+
+void NavigationServer::addDepthCameraData(const std::string& topic,
+                                          robot_sensor_msgs::DepthCameraData::ConstPtr data)
+{
+  if (data == nullptr)
+  {
+    return;  // Bỏ mẫu null thay vì cất một con trỏ rỗng để tầng sau vấp phải.
+  }
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+    depth_camera_data_[topic] = data;
+  }
+  sensors_.pushDepthCameraData(topic, data);
+}
+
+void NavigationServer::addOdometry(const std::string& /*odometry_name*/,
+                                   robot_nav_msgs::Odometry odometry)
+{
+  // CHỈ cất odometry. Không đụng twist_: xem publishCommand() — twist_ là lệnh phát ra, còn đây là
+  // vận tốc đo được. Trộn hai thứ đó là đưa cảm biến vào thẳng đường lệnh.
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  odometry_ = std::move(odometry);
+}
+
+// ================================================================================================
+// Đọc dữ liệu sensor
+// ================================================================================================
+
+robot_nav_msgs::OccupancyGrid NavigationServer::getStaticMap(const std::string& map_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return lookupOrDefault(static_maps_, map_name);
+}
+
+robot_sensor_msgs::LaserScan NavigationServer::getLaserScan(const std::string& laser_scan_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return lookupOrDefault(laser_scans_, laser_scan_name);
+}
+
+robot_sensor_msgs::PointCloud NavigationServer::getPointCloud(const std::string& point_cloud_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return lookupOrDefault(point_clouds_, point_cloud_name);
+}
+
+robot_sensor_msgs::PointCloud2 NavigationServer::getPointCloud2(
+    const std::string& point_cloud2_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return lookupOrDefault(point_cloud2s_, point_cloud2_name);
+}
+
+std::map<std::string, robot_nav_msgs::OccupancyGrid> NavigationServer::getAllStaticMaps()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return static_maps_;
+}
+
+std::map<std::string, robot_sensor_msgs::LaserScan> NavigationServer::getAllLaserScans()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return laser_scans_;
+}
+
+std::map<std::string, robot_sensor_msgs::PointCloud> NavigationServer::getAllPointClouds()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return point_clouds_;
+}
+
+std::map<std::string, robot_sensor_msgs::PointCloud2> NavigationServer::getAllPointCloud2s()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return point_cloud2s_;
+}
+
+bool NavigationServer::removeStaticMap(const std::string& map_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return static_maps_.erase(map_name) > 0;
+}
+
+bool NavigationServer::removeLaserScan(const std::string& laser_scan_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return laser_scans_.erase(laser_scan_name) > 0;
+}
+
+bool NavigationServer::removePointCloud(const std::string& point_cloud_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return point_clouds_.erase(point_cloud_name) > 0;
+}
+
+bool NavigationServer::removePointCloud2(const std::string& point_cloud2_name)
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return point_cloud2s_.erase(point_cloud2_name) > 0;
+}
+
+bool NavigationServer::removeAllStaticMaps()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  static_maps_.clear();
+  return true;
+}
+
+bool NavigationServer::removeAllLaserScans()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  laser_scans_.clear();
+  return true;
+}
+
+bool NavigationServer::removeAllPointClouds()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  point_clouds_.clear();
+  return true;
+}
+
+bool NavigationServer::removeAllPointCloud2s()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  point_cloud2s_.clear();
+  return true;
+}
+
+bool NavigationServer::removeAllData()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  static_maps_.clear();
+  laser_scans_.clear();
+  point_clouds_.clear();
+  point_cloud2s_.clear();
+  depth_camera_data_.clear();
+  return true;
+}
+
+// ================================================================================================
+// Sáu entry point di chuyển -> một NavigationRequest
+// ================================================================================================
+
+bool NavigationServer::submit(const NavigationRequest& request)
+{
+  std::string reason;
+  if (loop_.submit(request, reason))
+  {
+    last_reject_reason_.clear();
+    return true;
+  }
+
+  last_reject_reason_ = reason;
+  nav_feedback_->feed_back_str = "từ chối yêu cầu: " + reason;
+  return false;
+}
+
+bool NavigationServer::moveTo(const robot_geometry_msgs::PoseStamped& goal,
+                              double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kPosition;
+  request.goal = goal;
+  request.tolerance.xy = xy_goal_tolerance;
+  request.tolerance.yaw = yaw_goal_tolerance;
+  return submit(request);
+}
+
+bool NavigationServer::moveTo(const robot_protocol_msgs::Order& msg,
+                              const robot_geometry_msgs::PoseStamped& goal,
+                              double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kPosition;
+  request.goal = goal;
+  request.tolerance.xy = xy_goal_tolerance;
+  request.tolerance.yaw = yaw_goal_tolerance;
+  request.order = std::make_shared<robot_protocol_msgs::Order>(msg);
+  return submit(request);
+}
+
+bool NavigationServer::dockTo(const std::string& maker,
+                              const robot_geometry_msgs::PoseStamped& goal,
+                              double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kDocking;
+  request.goal = goal;
+  request.tolerance.xy = xy_goal_tolerance;
+  request.tolerance.yaw = yaw_goal_tolerance;
+  request.marker = maker;
+  return submit(request);
+}
+
+bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::string& marker,
+                              const robot_geometry_msgs::PoseStamped& goal,
+                              double xy_goal_tolerance, double yaw_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kDocking;
+  request.goal = goal;
+  request.tolerance.xy = xy_goal_tolerance;
+  request.tolerance.yaw = yaw_goal_tolerance;
+  request.marker = marker;
+  request.order = std::make_shared<robot_protocol_msgs::Order>(msg);
+  return submit(request);
+}
+
+bool NavigationServer::moveStraightTo(const robot_geometry_msgs::PoseStamped& goal,
+                                      double xy_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kGoStraight;
+  request.goal = goal;
+  request.tolerance.xy = xy_goal_tolerance;
+  return submit(request);
+}
+
+bool NavigationServer::rotateTo(const robot_geometry_msgs::PoseStamped& goal,
+                                double yaw_goal_tolerance)
+{
+  NavigationRequest request;
+  request.profile = MotionProfile::kRotate;
+  request.goal = goal;
+  request.tolerance.yaw = yaw_goal_tolerance;
+  return submit(request);
+}
+
+// ================================================================================================
+// Điều khiển vòng đời
+// ================================================================================================
+
+void NavigationServer::pause()
+{
+  loop_.requestPause();
+}
+
+void NavigationServer::resume()
+{
+  loop_.requestResume();
+}
+
+void NavigationServer::cancel()
+{
+  loop_.requestCancel();
+}
+
+bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear)
+{
+  // Không phải lệnh jog dù tên nghe như vậy: đây là TRẦN vận tốc, dấu chọn chiều, và host truyền
+  // xuống đây tốc độ đã bị tầng an toàn hạ (amr_control.cpp:561, 671-680).
+  if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z))
+  {
+    return false;
+  }
+
+  // Chỉ cất lại. Host gọi từ thread của nó (OPC-UA/VDA5050/ROS) còn ControllerPort không
+  // thread-safe, nên việc đẩy xuống controller thuộc về spinOnce() — xem pushHostInputsToController.
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  if (linear.x < 0.0)
+  {
+    pending_linear_backward_ = linear;
+    has_pending_linear_backward_ = true;
+  }
+  else
+  {
+    pending_linear_forward_ = linear;
+    has_pending_linear_forward_ = true;
+  }
+  return true;
+}
+
+bool NavigationServer::setTwistAngular(const robot_geometry_msgs::Vector3& angular)
+{
+  if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z))
+  {
+    return false;
+  }
+
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  pending_angular_ = angular;
+  has_pending_angular_ = true;
+  return true;
+}
+
+void NavigationServer::pushHostInputsToController()
+{
+  ControllerPort* controller = loop_.controllerPort();
+  if (controller == nullptr)
+  {
+    return;
+  }
+
+  robot_geometry_msgs::Vector3 linear_forward;
+  robot_geometry_msgs::Vector3 linear_backward;
+  robot_geometry_msgs::Vector3 angular;
+  bool push_forward = false;
+  bool push_backward = false;
+  bool push_angular = false;
+  robot_geometry_msgs::Twist velocity;
+
+  {
+    std::lock_guard<std::mutex> lock(data_mutex_);
+
+    push_forward = has_pending_linear_forward_;
+    push_backward = has_pending_linear_backward_;
+    push_angular = has_pending_angular_;
+    linear_forward = pending_linear_forward_;
+    linear_backward = pending_linear_backward_;
+    angular = pending_angular_;
+    has_pending_linear_forward_ = false;
+    has_pending_linear_backward_ = false;
+    has_pending_angular_ = false;
+
+    velocity = odometry_.twist.twist;
+  }
+
+  // Đẩy xuống NGOÀI lock: controller là plugin bên thứ ba, thời gian chạy của nó không được phép
+  // chặn các getter mà host đang gọi.
+  if (push_forward)
+  {
+    controller->setTwistLinear(linear_forward);
+  }
+  if (push_backward)
+  {
+    controller->setTwistLinear(linear_backward);
+  }
+  if (push_angular)
+  {
+    controller->setTwistAngular(angular);
+  }
+
+  controller->setMeasuredVelocity(velocity);
+}
+
+// ================================================================================================
+// Đọc trạng thái
+// ================================================================================================
+
+bool NavigationServer::getRobotPose(robot_geometry_msgs::PoseStamped& pose)
+{
+  PosePort* port = loop_.posePort();
+  if (port == nullptr)
+  {
+    return false;
+  }
+  // Hỏi thẳng cổng mỗi lần, không cache: mất TF phải nhìn thấy được ngay tại lời gọi này chứ không
+  // phải nhận về một pose cũ đã hết hạn.
+  return port->getRobotPose(pose);
+}
+
+bool NavigationServer::getRobotPose(robot_geometry_msgs::Pose2D& pose)
+{
+  robot_geometry_msgs::PoseStamped stamped;
+  if (!getRobotPose(stamped))
+  {
+    return false;
+  }
+  const robot_nav_2d_msgs::Pose2DStamped converted =
+      robot_nav_2d_utils::poseStampedToPose2D(stamped);
+  pose = converted.pose;
+  return true;
+}
+
+robot_nav_2d_msgs::Twist2DStamped NavigationServer::getTwist()
+{
+  std::lock_guard<std::mutex> lock(data_mutex_);
+  return twist_;
+}
+
+robot::move_base_core::NavFeedback* NavigationServer::getFeedback()
+{
+  return nav_feedback_.get();
+}
+
+robot::move_base_core::PlannerDataOutput NavigationServer::getGlobalData()
+{
+  return global_data_;
+}
+
+robot::move_base_core::PlannerDataOutput NavigationServer::getLocalData()
+{
+  return local_data_;
+}
+
+}  // namespace move_base2

src/navigation_state.cpp

@@ -0,0 +1,135 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — tên state và phân loại state.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/core/navigation_state.h>
+
+#include <move_base2/core/navigation_request.h>
+#include <move_base2/core/velocity_arbiter.h>
+#include <move_base2/ports/mission_port.h>
+#include <move_base2/ports/recovery_port.h>
+
+namespace move_base2
+{
+
+const char* toString(NavigationState state)
+{
+  switch (state)
+  {
+    case NavigationState::kIdle:
+      return "IDLE";
+    case NavigationState::kPlanning:
+      return "PLANNING";
+    case NavigationState::kControlling:
+      return "CONTROLLING";
+    case NavigationState::kRecovering:
+      return "RECOVERING";
+    case NavigationState::kExecutingActions:
+      return "EXECUTING_ACTIONS";
+    case NavigationState::kPaused:
+      return "PAUSED";
+    case NavigationState::kCancelling:
+      return "CANCELLING";
+    case NavigationState::kSucceeded:
+      return "SUCCEEDED";
+    case NavigationState::kAborted:
+      return "ABORTED";
+    case NavigationState::kCancelled:
+      return "CANCELLED";
+  }
+  return "UNKNOWN";
+}
+
+bool isTerminal(NavigationState state)
+{
+  return state == NavigationState::kSucceeded || state == NavigationState::kAborted ||
+         state == NavigationState::kCancelled;
+}
+
+bool mustBeStopped(NavigationState state)
+{
+  // Chỉ hai state được phép có vận tốc khác 0: kControlling (local planner lái) và kRecovering
+  // (recovery behavior lái). Mọi state còn lại là hàng rào an toàn — kể cả kExecutingActions:
+  // theo D8 action chạy khi robot đứng yên, action cần chuyển động phải là motion profile.
+  return state != NavigationState::kControlling && state != NavigationState::kRecovering;
+}
+
+const char* toString(MotionProfile profile)
+{
+  switch (profile)
+  {
+    case MotionProfile::kPosition:
+      return "position";
+    case MotionProfile::kDocking:
+      return "docking";
+    case MotionProfile::kGoStraight:
+      return "go_straight";
+    case MotionProfile::kRotate:
+      return "rotate";
+  }
+  return "unknown";
+}
+
+const char* toString(NavigationOutcome outcome)
+{
+  switch (outcome)
+  {
+    case NavigationOutcome::kSucceeded:
+      return "SUCCEEDED";
+    case NavigationOutcome::kFailed:
+      return "FAILED";
+    case NavigationOutcome::kCancelled:
+      return "CANCELLED";
+    case NavigationOutcome::kPreempted:
+      return "PREEMPTED";
+  }
+  return "UNKNOWN";
+}
+
+const char* toString(RecoveryTrigger trigger)
+{
+  switch (trigger)
+  {
+    case RecoveryTrigger::kPlanningFailed:
+      return "planning_failed";
+    case RecoveryTrigger::kControllingFailed:
+      return "controlling_failed";
+    case RecoveryTrigger::kOscillation:
+      return "oscillation";
+  }
+  return "unknown";
+}
+
+const char* toString(RecoveryOutputKind kind)
+{
+  switch (kind)
+  {
+    case RecoveryOutputKind::kNone:
+      return "none";
+    case RecoveryOutputKind::kVelocity:
+      return "velocity";
+    case RecoveryOutputKind::kPath:
+      return "path";
+  }
+  return "unknown";
+}
+
+const char* toString(VelocitySource source)
+{
+  switch (source)
+  {
+    case VelocitySource::kNone:
+      return "none";
+    case VelocitySource::kController:
+      return "controller";
+    case VelocitySource::kRecovery:
+      return "recovery";
+  }
+  return "unknown";
+}
+
+}  // namespace move_base2

src/runners/action_runner.cpp

@@ -0,0 +1,302 @@
+/*********************************************************************
+ * move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/runners/action_runner.h>
+
+#include <utility>
+
+#include <boost/dll/import.hpp>
+#include <boost/system/system_error.hpp>
+#include <yaml-cpp/yaml.h>
+
+#include <robot/robot.h>
+
+namespace move_base2
+{
+
+ActionRunner::~ActionRunner()
+{
+  // Handler phải chết TRƯỚC factory: factory là thứ giữ .so còn nạp.
+  active_ = nullptr;
+  by_type_.clear();
+  handlers_.clear();
+  factories_.clear();
+}
+
+void ActionRunner::setClock(ClockPort* clock)
+{
+  clock_ = clock;
+}
+
+void ActionRunner::setNamespace(const std::string& ns)
+{
+  namespace_ = ns;
+}
+
+bool ActionRunner::registerHandler(const ActionHandler::Ptr& handler)
+{
+  if (!handler)
+  {
+    robot::log_error("[move_base2] ActionRunner: handler null.");
+    return false;
+  }
+
+  const std::vector<std::string> types = handler->supportedActionTypes();
+  if (types.empty())
+  {
+    robot::log_error("[move_base2] ActionRunner: handler không khai actionType nào — sẽ không bao "
+                     "giờ được gọi.");
+    return false;
+  }
+
+  for (const std::string& type : types)
+  {
+    if (type.empty())
+    {
+      robot::log_error("[move_base2] ActionRunner: handler khai một actionType rỗng.");
+      return false;
+    }
+    if (by_type_.find(type) != by_type_.end())
+    {
+      // Hai handler cùng nhận một type thì việc định tuyến phụ thuộc thứ tự nạp — từ chối thay vì
+      // im lặng ghi đè.
+      robot::log_error("[move_base2] ActionRunner: actionType '%s' đã có handler khác đăng ký.",
+                       type.c_str());
+      return false;
+    }
+  }
+
+  handlers_.push_back(handler);
+  for (const std::string& type : types)
+  {
+    by_type_[type] = handler.get();
+  }
+  return true;
+}
+
+ActionHandler* ActionRunner::find(const std::string& action_type) const
+{
+  const auto it = by_type_.find(action_type);
+  return it == by_type_.end() ? nullptr : it->second;
+}
+
+std::vector<std::string> ActionRunner::supportedActionTypes() const
+{
+  std::vector<std::string> types;
+  types.reserve(by_type_.size());
+  for (const auto& entry : by_type_)
+  {
+    types.push_back(entry.first);
+  }
+  return types;
+}
+
+bool ActionRunner::loadOne(const std::string& name, const std::string& type,
+                           robot::NodeHandle& nh, const std::string& ns)
+{
+  robot::PluginLoaderHelper loader(nh);
+  const std::string library_path = loader.findLibraryPath(type);
+
+  if (library_path.empty())
+  {
+    robot::log_error("[move_base2] ActionRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+                     "'%s/library_path' trong YAML và sự tồn tại của file .so.",
+                     type.c_str(), type.c_str());
+    return false;
+  }
+
+  std::function<ActionHandler::Ptr()> factory;
+  try
+  {
+    factory = boost::dll::import_alias<ActionHandler::Ptr()>(
+        library_path, type, boost::dll::load_mode::append_decorations);
+  }
+  catch (const boost::system::system_error& ex)
+  {
+    robot::log_error("[move_base2] ActionRunner: không nạp được symbol '%s' từ '%s': %s",
+                     type.c_str(), library_path.c_str(), ex.what());
+    return false;
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ActionRunner: lỗi khi nạp '%s': %s", type.c_str(), ex.what());
+    return false;
+  }
+
+  ActionHandler::Ptr handler;
+  try
+  {
+    handler = factory();
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ActionRunner: factory của '%s' ném exception: %s", type.c_str(),
+                     ex.what());
+    return false;
+  }
+
+  if (!handler)
+  {
+    robot::log_error("[move_base2] ActionRunner: factory của '%s' trả về null.", type.c_str());
+    return false;
+  }
+
+  const std::string param_ns = ns.empty() ? name : ns + "/" + name;
+  robot::NodeHandle handler_nh(nh, param_ns);
+
+  if (!handler->configure(name, handler_nh))
+  {
+    robot::log_error("[move_base2] ActionRunner: '%s' (instance '%s') configure() thất bại.",
+                     type.c_str(), name.c_str());
+    return false;
+  }
+
+  if (!registerHandler(handler))
+  {
+    return false;
+  }
+
+  factories_.push_back(std::move(factory));
+
+  robot::log_info("[move_base2] ActionRunner: nạp '%s' (instance '%s').", type.c_str(),
+                  name.c_str());
+  return true;
+}
+
+bool ActionRunner::configure(robot::NodeHandle& nh)
+{
+  if (configured_)
+  {
+    robot::log_error("[move_base2] ActionRunner: configure() gọi lần thứ hai.");
+    return false;
+  }
+
+  if (clock_ == nullptr)
+  {
+    robot::log_error("[move_base2] ActionRunner: thiếu ClockPort — handler không có mốc timeout.");
+    return false;
+  }
+
+  const std::string key = namespace_.empty() ? std::string("handlers") : namespace_ + "/handlers";
+
+  YAML::Node list;
+  if (!nh.getParam(key, list) || !list.IsSequence())
+  {
+    // Không có handler nào là hợp lệ: hệ không có thiết bị thì mọi mission đều nav-only, và
+    // ControlLoop::submit đã từ chối yêu cầu mang action ngay tại cửa.
+    robot::log_warning("[move_base2] ActionRunner: '%s' không có danh sách handler — runtime sẽ "
+                       "từ chối mọi yêu cầu mang action.", key.c_str());
+    configured_ = true;
+    return true;
+  }
+
+  bool all_ok = true;
+
+  for (std::size_t i = 0; i < list.size(); ++i)
+  {
+    const YAML::Node& entry = list[i];
+
+    if (!entry.IsMap() || !entry["type"])
+    {
+      robot::log_error("[move_base2] ActionRunner: '%s[%zu]' thiếu khoá 'type'.", key.c_str(), i);
+      all_ok = false;
+      continue;
+    }
+
+    std::string type;
+    std::string name;
+    try
+    {
+      type = entry["type"].as<std::string>();
+      name = entry["name"] ? entry["name"].as<std::string>() : type;
+    }
+    catch (const YAML::Exception& ex)
+    {
+      robot::log_error("[move_base2] ActionRunner: '%s[%zu]' không đọc được: %s", key.c_str(), i,
+                       ex.what());
+      all_ok = false;
+      continue;
+    }
+
+    if (!loadOne(name, type, nh, namespace_))
+    {
+      all_ok = false;
+    }
+  }
+
+  configured_ = true;
+  return all_ok;
+}
+
+bool ActionRunner::start(const robot_protocol_msgs::Action& action)
+{
+  active_ = nullptr;
+  active_action_id_.clear();
+
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] ActionRunner: start() trước configure().");
+    return false;
+  }
+
+  if (action.actionType.empty())
+  {
+    robot::log_error("[move_base2] ActionRunner: action không có actionType.");
+    return false;
+  }
+
+  ActionHandler* handler = find(action.actionType);
+  if (handler == nullptr)
+  {
+    robot::log_error("[move_base2] ActionRunner: không handler nào nhận actionType '%s' (id '%s').",
+                     action.actionType.c_str(), action.actionId.c_str());
+    return false;
+  }
+
+  if (!handler->start(action, clock_->now()))
+  {
+    robot::log_warning("[move_base2] ActionRunner: handler từ chối khởi động action '%s' (id '%s').",
+                       action.actionType.c_str(), action.actionId.c_str());
+    return false;
+  }
+
+  active_ = handler;
+  active_action_id_ = action.actionId;
+  return true;
+}
+
+ActionTick ActionRunner::update()
+{
+  ActionTick tick;
+
+  if (active_ == nullptr)
+  {
+    // Contract nói update() chỉ được gọi sau start() trả true. Vẫn guard: lỗi thứ tự gọi phải thành
+    // "action này hỏng" chứ không phải dereference null.
+    tick.status = ActionTick::Status::kFailed;
+    tick.message = "update() khi không có action nào đang chạy";
+    return tick;
+  }
+
+  tick = active_->update(clock_->now());
+
+  if (tick.status != ActionTick::Status::kRunning)
+  {
+    active_ = nullptr;
+  }
+
+  return tick;
+}
+
+void ActionRunner::cancel()
+{
+  if (active_ != nullptr)
+  {
+    active_->cancel();
+    active_ = nullptr;
+  }
+}
+
+}  // namespace move_base2

src/runners/controller_runner.cpp

@@ -0,0 +1,408 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt ControllerRunner.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/runners/controller_runner.h>
+
+#include <cmath>
+#include <exception>
+#include <utility>
+
+#include <boost/dll/import.hpp>
+#include <boost/system/system_error.hpp>
+
+#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// [s] Giãn cách log cho cảnh báo phát sinh trong đường nóng — nhịp control loop, không được spam.
+constexpr double kHotPathLogThrottle = 5.0;
+
+bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
+{
+  return std::isfinite(twist.linear.x) && std::isfinite(twist.linear.y) &&
+         std::isfinite(twist.angular.z);
+}
+
+}  // namespace
+
+ControllerRunner::ControllerRunner() = default;
+ControllerRunner::~ControllerRunner() = default;
+
+bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* tf,
+                                 robot_costmap_2d::Costmap2DROBOT* costmap,
+                                 const std::string& initial_controller, std::string& error)
+{
+  if (configured_)
+  {
+    error = "ControllerRunner::configure() gọi lần thứ hai";
+    return false;
+  }
+
+  if (costmap == nullptr)
+  {
+    error = "ControllerRunner cần costmap local khác null";
+    return false;
+  }
+
+  nh_ = nh;
+  tf_ = tf;
+  costmap_ = costmap;
+  configured_ = true;
+
+  if (!initial_controller.empty() && !swapPlanner(initial_controller))
+  {
+    error = "không nạp được local planner khởi đầu '" + initial_controller + "'";
+    configured_ = false;
+    return false;
+  }
+
+  return true;
+}
+
+robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& name)
+{
+  const auto cached = controllers_.find(name);
+  if (cached != controllers_.end())
+  {
+    return cached->second.instance.get();
+  }
+
+  robot::PluginLoaderHelper loader(nh_);
+  const std::string library_path = loader.findLibraryPath(name);
+
+  if (library_path.empty())
+  {
+    robot::log_error("[move_base2] ControllerRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+                     "'%s/library_path' trong YAML và sự tồn tại của file .so trong devel/lib.\n",
+                     name.c_str(), name.c_str());
+    return nullptr;
+  }
+
+  Loaded loaded;
+
+  try
+  {
+    loaded.factory = boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
+        library_path, name, boost::dll::load_mode::append_decorations);
+  }
+  catch (const boost::system::system_error& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%s': %s\n",
+                     name.c_str(), library_path.c_str(), ex.what());
+    return nullptr;
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: lỗi khi nạp '%s': %s\n", name.c_str(),
+                     ex.what());
+    return nullptr;
+  }
+
+  try
+  {
+    loaded.instance = loaded.factory();
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném exception: %s\n",
+                     name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  if (!loaded.instance)
+  {
+    robot::log_error("[move_base2] ControllerRunner: factory của '%s' trả nullptr.\n", name.c_str());
+    return nullptr;
+  }
+
+  try
+  {
+    // Khác BaseGlobalPlanner: initialize ở đây trả void, nên không có cách nào biết plugin tự thấy
+    // mình hỏng. Chỉ chặn được exception.
+    loaded.instance->initialize(name, tf_, costmap_);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n",
+                     name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  const auto inserted = controllers_.emplace(name, std::move(loaded));
+  return inserted.first->second.instance.get();
+}
+
+void ControllerRunner::applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller)
+{
+  if (controller == nullptr)
+  {
+    return;
+  }
+
+  // Trần vận tốc thuộc về YÊU CẦU, không thuộc về instance planner. Instance mới nạp không biết gì
+  // về các trần host đã đặt trước đó — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an
+  // toàn cho phép, và không có dấu hiệu nào cả.
+  try
+  {
+    if (has_limit_linear_forward_)
+    {
+      controller->setTwistLinear(limit_linear_forward_);
+    }
+    if (has_limit_linear_backward_)
+    {
+      controller->setTwistLinear(limit_linear_backward_);
+    }
+    if (has_limit_angular_)
+    {
+      controller->setTwistAngular(limit_angular_);
+    }
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: lỗi khi áp lại trần vận tốc: %s\n", ex.what());
+  }
+}
+
+bool ControllerRunner::swapPlanner(const std::string& planner_name)
+{
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n");
+    return false;
+  }
+
+  if (planner_name.empty())
+  {
+    robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n");
+    return false;
+  }
+
+  if (planner_name == active_name_ && active_ != nullptr)
+  {
+    return true;  // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu.
+  }
+
+  robot_nav_core::BaseLocalPlanner* controller = acquire(planner_name);
+  if (controller == nullptr)
+  {
+    // Giữ nguyên controller đang chạy: bên gọi từ chối yêu cầu dựa vào giá trị trả về.
+    return false;
+  }
+
+  active_ = controller;
+  active_name_ = planner_name;
+  applyPendingLimits(active_);
+
+  robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n",
+                  planner_name.c_str());
+  return true;
+}
+
+void ControllerRunner::setTolerance(double xy_m, double yaw_rad)
+{
+  if (!configured_)
+  {
+    return;
+  }
+
+  // Interface gen-1 không có hàm đặt sai số; bản cũ ghi vào param rồi để planner tự đọc lại. Kênh
+  // gián tiếp này được giữ nguyên để không đổi hành vi của các planner đang chạy — nhưng planner
+  // nào chỉ đọc param lúc initialize sẽ KHÔNG thấy giá trị mới. Xem doc của lớp.
+  nh_.setParam("xy_goal_tolerance", xy_m);
+  nh_.setParam("yaw_goal_tolerance", yaw_rad);
+}
+
+bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan)
+{
+  if (!configured_ || active_ == nullptr)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: setPlan() khi chưa có controller.\n");
+    return false;
+  }
+
+  if (plan.empty())
+  {
+    // Plan rỗng lọt xuống sẽ thành front()/back() trên vector rỗng bên trong planner.
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: từ chối plan rỗng.\n");
+    return false;
+  }
+
+  try
+  {
+    return active_->setPlan(plan);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: '%s' ném exception trong setPlan: "
+                              "%s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
+{
+  cmd = robot_geometry_msgs::Twist();
+
+  if (!configured_ || active_ == nullptr)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: computeVelocityCommands() khi chưa "
+                              "có controller.\n");
+    return false;
+  }
+
+  robot_geometry_msgs::Twist result;
+  bool ok = false;
+
+  try
+  {
+    ok = active_->computeVelocityCommands(measured_velocity_, result);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: '%s' ném exception khi tính lệnh: "
+                              "%s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+
+  if (!ok)
+  {
+    return false;
+  }
+
+  if (!isFiniteTwist(result))
+  {
+    // VelocityArbiter cũng chặn NaN/Inf, nhưng chặn ngay tại nguồn cho biết ĐÚNG plugin nào đang
+    // trả dữ liệu hỏng — arbiter chỉ thấy một con số vô nghĩa không rõ từ đâu.
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: '%s' trả lệnh chứa NaN/Inf.\n",
+                              active_name_.c_str());
+    return false;
+  }
+
+  cmd = result;
+  return true;
+}
+
+bool ControllerRunner::isGoalReached()
+{
+  if (!configured_ || active_ == nullptr)
+  {
+    return false;
+  }
+
+  try
+  {
+    return active_->isGoalReached();
+  }
+  catch (const std::exception& ex)
+  {
+    // Trả false: "chưa tới đích" là phía an toàn — báo nhầm đã tới sẽ kết thúc chặng đường sớm và
+    // robot dừng ở chỗ không phải đích.
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: '%s' ném exception trong "
+                              "isGoalReached: %s\n", active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity)
+{
+  if (!isFiniteTwist(velocity))
+  {
+    // Giữ giá trị cũ thay vì đưa NaN vào hàm tính lệnh — nhiều local planner dùng nó làm mốc giới
+    // hạn gia tốc, và NaN ở đó lan ra toàn bộ cost function.
+    robot::log_error_throttle(kHotPathLogThrottle,
+                              "[move_base2] ControllerRunner: bỏ vận tốc đo được chứa NaN/Inf.\n");
+    return;
+  }
+  measured_velocity_ = velocity;
+}
+
+bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear)
+{
+  if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z))
+  {
+    robot::log_error("[move_base2] ControllerRunner: trần vận tốc thẳng chứa NaN/Inf, bỏ qua.\n");
+    return false;
+  }
+
+  // Dấu chọn chiều — xem doc của ControllerPort::setTwistLinear. Nhớ cả hai chiều riêng để
+  // swapPlanner còn áp lại được lên instance mới.
+  if (linear.x < 0.0)
+  {
+    limit_linear_backward_ = linear;
+    has_limit_linear_backward_ = true;
+  }
+  else
+  {
+    limit_linear_forward_ = linear;
+    has_limit_linear_forward_ = true;
+  }
+
+  if (active_ == nullptr)
+  {
+    // Host đặt trần trước khi controller được nạp là chuyện bình thường: thứ tự khởi tạo không do
+    // move_base2 quyết. Đã nhớ lại, sẽ áp khi có controller.
+    return true;
+  }
+
+  try
+  {
+    return active_->setTwistLinear(linear);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n",
+                     active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angular)
+{
+  if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z))
+  {
+    robot::log_error("[move_base2] ControllerRunner: trần vận tốc góc chứa NaN/Inf, bỏ qua.\n");
+    return false;
+  }
+
+  limit_angular_ = angular;
+  has_limit_angular_ = true;
+
+  if (active_ == nullptr)
+  {
+    return true;
+  }
+
+  try
+  {
+    return active_->setTwistAngular(angular);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n",
+                     active_name_.c_str(), ex.what());
+    return false;
+  }
+}
+
+std::string ControllerRunner::activeController() const
+{
+  return active_ != nullptr ? active_name_ : std::string();
+}
+
+}  // namespace move_base2

src/runners/planner_runner.cpp

@@ -0,0 +1,395 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt PlannerRunner.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/runners/planner_runner.h>
+
+#include <cmath>
+#include <exception>
+#include <utility>
+
+#include <boost/dll/import.hpp>
+#include <boost/system/system_error.hpp>
+
+#include <robot/plugin_loader_helper.h>
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// @brief Pose có dùng được để lập plan không — NaN/Inf lọt vào SBPL là hỏng ở tầng khó truy nhất.
+bool isFinitePose(const robot_geometry_msgs::PoseStamped& pose)
+{
+  return std::isfinite(pose.pose.position.x) && std::isfinite(pose.pose.position.y) &&
+         std::isfinite(pose.pose.orientation.z) && std::isfinite(pose.pose.orientation.w);
+}
+
+}  // namespace
+
+PlannerRunner::PlannerRunner() = default;
+
+PlannerRunner::~PlannerRunner()
+{
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    shutdown_ = true;
+    discard_ = true;
+  }
+  cv_.notify_all();
+
+  if (thread_.joinable())
+  {
+    // Chờ có chủ đích: plugin là hộp đen nạp lúc chạy, không có đường cắt ngang một phép tính đang
+    // chạy. Detach thay vì join sẽ để thread chạm vào `planning_`/`handoff_` sau khi chúng đã bị
+    // huỷ — hỏng ở chỗ không thể truy được.
+    thread_.join();
+  }
+}
+
+bool PlannerRunner::configure(const robot::NodeHandle& nh,
+                              robot_costmap_2d::Costmap2DROBOT* costmap,
+                              const std::string& initial_planner, std::string& error)
+{
+  if (configured_)
+  {
+    error = "PlannerRunner::configure() gọi lần thứ hai";
+    return false;
+  }
+
+  if (costmap == nullptr)
+  {
+    // Không có costmap thì `BaseGlobalPlanner::initialize` nhận nullptr và mọi plugin tự quyết định
+    // làm gì với nó — thường là sập. Chặn ở đây, nơi còn nói được lý do.
+    error = "PlannerRunner cần costmap global khác null";
+    return false;
+  }
+
+  nh_ = nh;
+  costmap_ = costmap;
+  configured_ = true;
+
+  if (!initial_planner.empty() && !swapPlanner(initial_planner))
+  {
+    error = "không nạp được global planner khởi đầu '" + initial_planner + "'";
+    configured_ = false;
+    return false;
+  }
+
+  thread_ = std::thread(&PlannerRunner::threadBody, this);
+  return true;
+}
+
+// ================================================================================================
+// Nạp plugin — chỉ control thread chạm
+// ================================================================================================
+
+robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& name)
+{
+  const auto cached = planners_.find(name);
+  if (cached != planners_.end())
+  {
+    return cached->second.instance.get();
+  }
+
+  robot::PluginLoaderHelper loader(nh_);
+  const std::string library_path = loader.findLibraryPath(name);
+
+  if (library_path.empty())
+  {
+    robot::log_error("[move_base2] PlannerRunner: không tìm được thư viện cho '%s' — kiểm khoá "
+                     "'%s/library_path' trong YAML và sự tồn tại của file .so trong devel/lib.\n",
+                     name.c_str(), name.c_str());
+    return nullptr;
+  }
+
+  Loaded loaded;
+
+  try
+  {
+    loaded.factory = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
+        library_path, name, boost::dll::load_mode::append_decorations);
+  }
+  catch (const boost::system::system_error& ex)
+  {
+    robot::log_error("[move_base2] PlannerRunner: không nạp được symbol '%s' từ '%s': %s\n",
+                     name.c_str(), library_path.c_str(), ex.what());
+    return nullptr;
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] PlannerRunner: lỗi khi nạp '%s': %s\n", name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  try
+  {
+    loaded.instance = loaded.factory();
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] PlannerRunner: factory của '%s' ném exception: %s\n",
+                     name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  if (!loaded.instance)
+  {
+    robot::log_error("[move_base2] PlannerRunner: factory của '%s' trả nullptr.\n", name.c_str());
+    return nullptr;
+  }
+
+  bool initialized = false;
+  try
+  {
+    initialized = loaded.instance->initialize(name, costmap_);
+  }
+  catch (const std::exception& ex)
+  {
+    robot::log_error("[move_base2] PlannerRunner: initialize() của '%s' ném exception: %s\n",
+                     name.c_str(), ex.what());
+    return nullptr;
+  }
+
+  if (!initialized)
+  {
+    // Bản cũ chỉ log rồi đi tiếp với một planner chưa khởi tạo. Ở đây coi là thất bại: một planner
+    // báo "tôi chưa sẵn sàng" mà vẫn được gọi makePlan là đường dẫn tới hành vi không xác định.
+    robot::log_error("[move_base2] PlannerRunner: '%s' báo initialize() thất bại.\n", name.c_str());
+    return nullptr;
+  }
+
+  // Chỉ đưa vào cache khi đã khởi tạo xong — cache một instance hỏng nghĩa là mọi lần thử lại sau
+  // đều nhận lại đúng cái hỏng đó mà không báo gì.
+  const auto inserted = planners_.emplace(name, std::move(loaded));
+  return inserted.first->second.instance.get();
+}
+
+bool PlannerRunner::swapPlanner(const std::string& planner_name)
+{
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] PlannerRunner: swapPlanner() trước configure().\n");
+    return false;
+  }
+
+  if (planner_name.empty())
+  {
+    robot::log_error("[move_base2] PlannerRunner: tên planner rỗng.\n");
+    return false;
+  }
+
+  if (planner_name == active_name_)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (active_ != nullptr)
+    {
+      return true;  // Đã đúng planner; không log để khỏi spam ở cửa vào mỗi yêu cầu.
+    }
+  }
+
+  robot_nav_core::BaseGlobalPlanner* planner = acquire(planner_name);
+  if (planner == nullptr)
+  {
+    // Giữ nguyên planner đang chạy. Bên gọi từ chối yêu cầu dựa vào giá trị trả về; chuyển sang
+    // trạng thái "không có planner" ở đây sẽ làm hỏng luôn cả yêu cầu đang chạy dở.
+    return false;
+  }
+
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    // Lượt đang chạy thuộc về planner cũ — kết quả của nó không còn nghĩa. Không cắt ngang được
+    // phép tính, chỉ đánh dấu vứt kết quả. Con trỏ planner cũ vẫn hợp lệ vì cache không xoá entry.
+    if (running_ || pending_)
+    {
+      discard_ = true;
+    }
+    active_ = planner;
+  }
+
+  active_name_ = planner_name;
+  robot::log_info("[move_base2] PlannerRunner: global planner đang dùng là '%s'.\n",
+                  planner_name.c_str());
+  return true;
+}
+
+std::string PlannerRunner::activePlanner() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return active_ != nullptr ? active_name_ : std::string();
+}
+
+// ================================================================================================
+// Vòng đời một lượt lập plan
+// ================================================================================================
+
+bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start,
+                              const robot_geometry_msgs::PoseStamped& goal,
+                              const robot_protocol_msgs::Order* order, std::uint64_t tag)
+{
+  if (!configured_)
+  {
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() trước configure().\n");
+    return false;
+  }
+
+  if (!isFinitePose(start) || !isFinitePose(goal))
+  {
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: start hoặc goal chứa NaN/Inf, "
+                                   "không khởi động lượt lập plan.\n");
+    return false;
+  }
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (active_ == nullptr)
+  {
+    robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() khi chưa có planner.\n");
+    return false;
+  }
+
+  if (pending_ || running_)
+  {
+    return false;  // Đã có lượt đang chạy; bên gọi phải chờ hoặc huỷ trước.
+  }
+
+  request_start_ = start;
+  request_goal_ = goal;
+  // Sao chép Order: con trỏ chỉ hợp lệ trong lời gọi này, còn lượt lập plan sống lâu hơn thế.
+  request_order_ = (order != nullptr) ? std::make_shared<robot_protocol_msgs::Order>(*order)
+                                      : nullptr;
+  request_tag_ = tag;
+
+  pending_ = true;
+  discard_ = false;
+  has_result_ = false;
+
+  cv_.notify_one();
+  return true;
+}
+
+bool PlannerRunner::isPlanning() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return pending_ || running_;
+}
+
+bool PlannerRunner::pollPlan(PlanResult& result)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (!has_result_)
+  {
+    return false;
+  }
+
+  has_result_ = false;
+  result.tag = result_tag_;
+  result.succeeded = result_ok_;
+
+  // Hoán vị chứ không copy: vector cũ của bên gọi quay lại làm hộp thư và giữ nguyên capacity, nên
+  // ở trạng thái ổn định không có lần cấp phát nào cho việc bàn giao plan.
+  result.plan.swap(handoff_);
+  handoff_.clear();
+  return true;
+}
+
+void PlannerRunner::cancelPlan()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (pending_ || running_)
+  {
+    discard_ = true;
+  }
+  // Kết quả đã nằm sẵn trong hộp thư cũng bỏ luôn: bên gọi vừa nói nó không còn cần plan này.
+  has_result_ = false;
+  handoff_.clear();
+}
+
+// ================================================================================================
+// Thread planner
+// ================================================================================================
+
+void PlannerRunner::threadBody()
+{
+  std::unique_lock<std::mutex> lock(mutex_);
+
+  while (true)
+  {
+    cv_.wait(lock, [this] { return shutdown_ || pending_; });
+
+    if (shutdown_)
+    {
+      return;
+    }
+
+    // Chụp lại yêu cầu rồi NHẢ MUTEX trước khi gọi plugin. Đây là điểm mấu chốt của cả mô hình:
+    // control thread không bao giờ phải chờ một lượt lập plan.
+    robot_nav_core::BaseGlobalPlanner* planner = active_;
+    const robot_geometry_msgs::PoseStamped start = request_start_;
+    const robot_geometry_msgs::PoseStamped goal = request_goal_;
+    const std::shared_ptr<robot_protocol_msgs::Order> order = request_order_;
+    const std::uint64_t tag = request_tag_;
+
+    pending_ = false;
+    running_ = true;
+
+    lock.unlock();
+
+    planning_.clear();
+    bool ok = false;
+
+    try
+    {
+      // Hai overload của interface gốc gộp lại: "có Order hay không" là một nhánh, không phải hai
+      // contract. Plugin nào không hiểu Order thì overload mặc định của nó tự lo.
+      ok = (order != nullptr) ? planner->makePlan(*order, start, goal, planning_)
+                              : planner->makePlan(start, goal, planning_);
+    }
+    catch (const std::exception& ex)
+    {
+      // Plugin bên thứ ba ném ra thì đây là biên duy nhất chặn được — exception thoát khỏi thân
+      // thread là std::terminate, tức mất cả tiến trình navigation vì một lượt lập plan hỏng.
+      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin ném exception khi lập "
+                                     "plan: %s\n", ex.what());
+      ok = false;
+    }
+    catch (...)
+    {
+      robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin ném exception lạ khi lập "
+                                     "plan.\n");
+      ok = false;
+    }
+
+    if (!ok || planning_.empty())
+    {
+      // Contract của PlannerPort: thành công nghĩa là plan KHÔNG rỗng. Một số plugin trả true kèm
+      // vector rỗng; quy về thất bại ngay tại đây để tầng trên không gọi front()/back() trên nó.
+      planning_.clear();
+      ok = false;
+    }
+
+    lock.lock();
+    running_ = false;
+
+    if (discard_)
+    {
+      // Lượt này đã bị huỷ hoặc planner đã bị đổi giữa chừng. Vứt lặng lẽ — không phải lỗi.
+      discard_ = false;
+      planning_.clear();
+      continue;
+    }
+
+    planning_.swap(handoff_);
+    result_tag_ = tag;
+    result_ok_ = ok;
+    has_result_ = true;
+  }
+}
+
+}  // namespace move_base2

src/runners/recovery_runner.cpp

@@ -0,0 +1,253 @@
+/*********************************************************************
+ * move_base2 — hiện thực RecoveryPort bằng recovery_core.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/runners/recovery_runner.h>
+
+#include <utility>
+
+#include <robot/robot.h>
+
+namespace move_base2
+{
+namespace
+{
+
+/// Dịch lý do vào recovery sang enum của recovery_core. `switch` đầy đủ để compiler bắt được ngay
+/// khi một bên thêm giá trị mới — đó là toàn bộ lý do không dùng cast số.
+recovery_core::RecoveryTrigger toCoreTrigger(RecoveryTrigger trigger)
+{
+  switch (trigger)
+  {
+    case RecoveryTrigger::kPlanningFailed:
+      return recovery_core::RecoveryTrigger::kPlanningFailed;
+    case RecoveryTrigger::kControllingFailed:
+      return recovery_core::RecoveryTrigger::kControllingFailed;
+    case RecoveryTrigger::kOscillation:
+      return recovery_core::RecoveryTrigger::kOscillation;
+  }
+  return recovery_core::RecoveryTrigger::kUnspecified;
+}
+
+RecoveryOutputKind toPortKind(recovery_core::RecoveryOutputType kind)
+{
+  switch (kind)
+  {
+    case recovery_core::RecoveryOutputType::kNone:
+      return RecoveryOutputKind::kNone;
+    case recovery_core::RecoveryOutputType::kVelocity:
+      return RecoveryOutputKind::kVelocity;
+    case recovery_core::RecoveryOutputType::kPath:
+      return RecoveryOutputKind::kPath;
+  }
+  return RecoveryOutputKind::kNone;
+}
+
+}  // namespace
+
+void RecoveryRunner::setDeps(const Deps& deps)
+{
+  deps_ = deps;
+  pose_bridge_.setPort(deps_.pose);
+  collision_.setCostmap(deps_.local_costmap);
+}
+
+void RecoveryRunner::setCostmaps(robot_costmap_2d::Costmap2DROBOT* local,
+                                 robot_costmap_2d::Costmap2DROBOT* global)
+{
+  deps_.local_costmap = local;
+  deps_.global_costmap = global;
+  collision_.setCostmap(local);
+}
+
+void RecoveryRunner::setNamespace(const std::string& ns)
+{
+  namespace_ = ns;
+}
+
+void RecoveryRunner::setPlanSource(PlanSource source)
+{
+  plan_bridge_.setSource(std::move(source));
+}
+
+void RecoveryRunner::refreshContext()
+{
+  // Trỏ lại mỗi lượt thay vì tin bản cache từ configure(): con trỏ costmap là non-owning và có thể
+  // bị thay khi runtime dựng lại costmap. Cache nó chính là nguyên nhân lỗi double-free đã ghi nhận
+  // trong workspace.
+  collision_.setCostmap(deps_.local_costmap);
+
+  ctx_.pose = &pose_bridge_;
+  ctx_.collision = &collision_;
+  ctx_.plan = &plan_bridge_;
+  ctx_.local_costmap = deps_.local_costmap;
+  ctx_.global_costmap = deps_.global_costmap;
+}
+
+bool RecoveryRunner::configure(robot::NodeHandle& nh)
+{
+  if (configured_)
+  {
+    robot::log_error("[move_base2] RecoveryRunner: configure() gọi lần thứ hai.");
+    return false;
+  }
+
+  if (deps_.clock == nullptr || deps_.pose == nullptr)
+  {
+    robot::log_error("[move_base2] RecoveryRunner: thiếu ClockPort hoặc PosePort.");
+    return false;
+  }
+
+  refreshContext();
+
+  const bool all_ok = registry_.loadFromConfig(nh, namespace_, ctx_);
+
+  if (registry_.size() == 0)
+  {
+    robot::log_error("[move_base2] RecoveryRunner: không nạp được behavior nào từ namespace '%s' — "
+                     "runtime sẽ không có đường phục hồi.", namespace_.c_str());
+    return false;
+  }
+
+  configured_ = true;
+
+  if (!all_ok)
+  {
+    // Một số behavior hỏng nhưng phần còn lại dùng được: giữ chúng lại và báo false để bên gọi
+    // quyết định (chạy tiếp với ít đường phục hồi hơn, hay dừng khởi động).
+    robot::log_warning("[move_base2] RecoveryRunner: nạp được %zu behavior, một số entry bị bỏ.",
+                       registry_.size());
+    return false;
+  }
+
+  robot::log_info("[move_base2] RecoveryRunner: nạp %zu recovery behavior từ '%s'.",
+                  registry_.size(), namespace_.c_str());
+  return true;
+}
+
+std::size_t RecoveryRunner::behaviorCount() const
+{
+  return registry_.size();
+}
+
+RecoveryOutputKind RecoveryRunner::outputKind(std::size_t index) const
+{
+  const recovery_core::RecoveryBehavior* behavior = registry_.at(index);
+  // Index sai -> kNone: không cấp quyền phát vận tốc cho thứ không biết là gì.
+  return behavior == nullptr ? RecoveryOutputKind::kNone : toPortKind(behavior->outputKind());
+}
+
+std::string RecoveryRunner::behaviorName(std::size_t index) const
+{
+  return registry_.nameAt(index);
+}
+
+bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger)
+{
+  active_ = nullptr;
+
+  if (!configured_)
+  {
+    robot::log_error("[move_base2] RecoveryRunner: start() trước configure().");
+    return false;
+  }
+
+  recovery_core::RecoveryBehavior* behavior = registry_.at(index);
+  if (behavior == nullptr)
+  {
+    robot::log_error("[move_base2] RecoveryRunner: index %zu ngoài dải (%zu behavior).", index,
+                     registry_.size());
+    return false;
+  }
+
+  refreshContext();
+
+  recovery_core::RecoveryGoal goal;
+  goal.trigger = toCoreTrigger(trigger);
+  // Không đặt angle/distance: để behavior dùng default đã cấu hình của nó. Lõi chưa có nguồn thông
+  // tin nào để chọn góc/quãng tốt hơn config; khi có (ví dụ hình học vật cản), đặt vào đây.
+
+  if (!behavior->start(goal, deps_.clock->now()))
+  {
+    robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' từ chối khởi động (%s).",
+                       registry_.nameAt(index).c_str(), toString(trigger));
+    return false;
+  }
+
+  active_ = behavior;
+  return true;
+}
+
+RecoveryTick RecoveryRunner::update()
+{
+  RecoveryTick tick;
+
+  if (active_ == nullptr)
+  {
+    // Contract nói update() chỉ được gọi sau start() trả true. Vẫn guard: state machine hỏng thì
+    // phải thành "recovery này thất bại" chứ không phải dereference null.
+    tick.status = RecoveryTick::Status::kFailed;
+    tick.message = "update() khi không có behavior nào đang chạy";
+    return tick;
+  }
+
+  refreshContext();
+  return toTick(active_->update(deps_.clock->now()));
+}
+
+void RecoveryRunner::cancel()
+{
+  if (active_ != nullptr)
+  {
+    active_->cancel();
+  }
+}
+
+RecoveryTick RecoveryRunner::toTick(const recovery_core::RecoveryResult& result) const
+{
+  RecoveryTick tick;
+
+  switch (result.status)
+  {
+    case recovery_core::RecoveryStatus::kRunning:
+      tick.status = RecoveryTick::Status::kRunning;
+      break;
+    case recovery_core::RecoveryStatus::kSucceeded:
+      tick.status = RecoveryTick::Status::kSucceeded;
+      break;
+    case recovery_core::RecoveryStatus::kIdle:
+      // Behavior chưa start mà đã bị tick — lỗi thứ tự gọi, không phải trạng thái bình thường.
+      tick.status = RecoveryTick::Status::kFailed;
+      break;
+    case recovery_core::RecoveryStatus::kCancelled:
+      // Nhánh này KHÔNG đạt tới được với state machine hiện tại: sau cancel() nó chuyển sang
+      // CANCELLING, nơi tick_recovery bị ép false. Giữ nhánh lại làm hàng rào nếu sau này ai đó nới
+      // điều kiện tick — bỏ đi thì kCancelled sẽ rơi vào default và im lặng thành kRunning.
+      tick.status = RecoveryTick::Status::kFailed;
+      break;
+    case recovery_core::RecoveryStatus::kFailed:
+      tick.status = RecoveryTick::Status::kFailed;
+      break;
+  }
+
+  if (const robot_geometry_msgs::Twist* cmd = result.velocity())
+  {
+    tick.has_velocity = true;
+    tick.cmd = *cmd;
+  }
+
+  if (const robot_nav_msgs::Path* path = result.pathOut())
+  {
+    if (!path->poses.empty())
+    {
+      tick.has_path = true;
+      tick.path = path->poses;
+    }
+  }
+
+  tick.message = result.message;
+  return tick;
+}
+
+}  // namespace move_base2

src/state_machine.cpp

@@ -0,0 +1,578 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt state machine. Bảng chuyển đầy đủ, không I/O, không logging.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/core/state_machine.h>
+
+#include <sstream>
+
+namespace move_base2
+{
+
+// ================================================================================================
+// StateMachineConfig
+// ================================================================================================
+
+bool StateMachineConfig::validate(std::string& error) const
+{
+  // Các ngưỡng thời gian được phép <= 0 với nghĩa "tắt", nên không kiểm dấu ở đây. Thứ phải chặn là
+  // giá trị vô nghĩa: khoảng cách chống quẩn âm, và bật chống quẩn mà không cho khoảng cách nào.
+  if (oscillation_distance < 0.0)
+  {
+    error = "oscillation_distance phải >= 0 [m]";
+    return false;
+  }
+  if (oscillation_timeout > 0.0 && oscillation_distance <= 0.0)
+  {
+    error = "bật oscillation_timeout thì oscillation_distance phải > 0 [m], nếu không mọi cycle "
+            "đều bị coi là quẩn";
+    return false;
+  }
+  if (planner_patience <= 0.0 && max_planning_retries < 0)
+  {
+    // Từ khi lập plan chạy trên thread riêng, hai tham số này là thứ DUY NHẤT phát hiện được planner
+    // treo. Tắt cả hai nghĩa là một plugin không bao giờ trả lời sẽ giữ robot ở PLANNING vĩnh viễn,
+    // im lặng, và state machine tin rằng mọi thứ bình thường. Ở chế độ đồng bộ trước đây điều này
+    // vô hại hơn nhiều vì planner treo làm treo luôn control loop — hỏng thì thấy ngay.
+    error = "planner_patience <= 0 [s] và max_planning_retries < 0 cùng lúc: không có gì phát hiện "
+            "được planner treo; đặt ít nhất một trong hai";
+    return false;
+  }
+  if (recovery_enabled && recovery_behavior_count == 0)
+  {
+    // Không phải lỗi cấu hình chết người, nhưng để im lặng thì lúc chạy sẽ ABORTED ngay ở lỗi đầu
+    // tiên mà không ai hiểu vì sao. Bắt buộc khai báo tường minh recovery_enabled = false.
+    error = "recovery_enabled = true nhưng recovery_behavior_count = 0; đặt recovery_enabled = "
+            "false nếu thực sự không muốn có recovery";
+    return false;
+  }
+  return true;
+}
+
+std::string StateMachineConfig::describe() const
+{
+  std::ostringstream out;
+  out << "StateMachineConfig:\n";
+  out << "  planner_patience      : " << planner_patience << " s"
+      << (planner_patience > 0.0 ? "" : " (tắt)") << '\n';
+  out << "  controller_patience   : " << controller_patience << " s"
+      << (controller_patience > 0.0 ? "" : " (tắt)") << '\n';
+  out << "  oscillation_timeout   : " << oscillation_timeout << " s"
+      << (oscillation_timeout > 0.0 ? "" : " (tắt)") << '\n';
+  out << "  action_patience       : " << action_patience << " s"
+      << (action_patience > 0.0 ? "" : " (tắt — handler tự timeout)") << '\n';
+  out << "  oscillation_distance  : " << oscillation_distance << " m\n";
+  out << "  max_planning_retries  : " << max_planning_retries
+      << (max_planning_retries < 0 ? " (không giới hạn)" : "") << '\n';
+  out << "  recovery_enabled      : " << (recovery_enabled ? "true" : "false") << '\n';
+  out << "  recovery_behavior_cnt : " << recovery_behavior_count << '\n';
+  return out.str();
+}
+
+// ================================================================================================
+// StateMachine
+// ================================================================================================
+
+bool StateMachine::configure(const StateMachineConfig& config, std::string& error)
+{
+  if (!config.validate(error))
+  {
+    initialized_ = false;
+    return false;
+  }
+
+  config_ = config;
+  initialized_ = true;
+  reset();
+  return true;
+}
+
+void StateMachine::reset()
+{
+  state_ = NavigationState::kIdle;
+  state_before_pause_ = NavigationState::kIdle;
+  state_entered_at_ = robot::Time();
+  last_valid_plan_ = robot::Time();
+  last_valid_control_ = robot::Time();
+  last_oscillation_reset_ = robot::Time();
+  recovery_index_ = 0;
+  planning_retries_ = 0;
+  request_has_goal_ = true;
+  action_count_ = 0;
+  action_index_ = 0;
+  action_started_at_ = robot::Time();
+}
+
+double StateMachine::secondsInState(const robot::Time& now) const
+{
+  return (now - state_entered_at_).toSec();
+}
+
+void StateMachine::enter(NavigationState next, const robot::Time& now, const char* reason,
+                         StateMachineOutput& out)
+{
+  if (next != state_)
+  {
+    state_ = next;
+    state_entered_at_ = now;
+    out.state_changed = true;
+  }
+  out.state = state_;
+  out.reason = reason;
+}
+
+void StateMachine::beginPlanningCycle(const robot::Time& now)
+{
+  last_valid_plan_ = now;
+  planning_retries_ = 0;
+}
+
+void StateMachine::escalateToRecovery(RecoveryTrigger trigger, const robot::Time& now,
+                                      const char* reason, StateMachineOutput& out)
+{
+  if (!config_.recovery_enabled || recovery_index_ >= config_.recovery_behavior_count)
+  {
+    finish(NavigationState::kAborted, NavigationOutcome::kFailed, now,
+           "hết recovery behavior khả dụng", out);
+    return;
+  }
+
+  out.start_recovery = true;
+  out.recovery_index = recovery_index_;
+  out.recovery_trigger = trigger;
+  enter(NavigationState::kRecovering, now, reason, out);
+}
+
+void StateMachine::finish(NavigationState terminal, NavigationOutcome outcome,
+                          const robot::Time& now, const char* reason, StateMachineOutput& out)
+{
+  // Cờ này bật đúng tại cycle bước vào state terminal, và state terminal chỉ tồn tại một cycle
+  // (cycle sau đã về kIdle). Đó là toàn bộ cơ chế giữ bất biến "báo kết quả đúng một lần".
+  out.report_outcome = true;
+  out.outcome = outcome;
+  out.stop_planner = true;
+  enter(terminal, now, reason, out);
+}
+
+StateMachineOutput StateMachine::update(const StateMachineInput& in)
+{
+  StateMachineOutput out;
+  out.state = state_;
+  out.recovery_index = recovery_index_;
+
+  if (!initialized_)
+  {
+    // Guard bắt buộc: không bao giờ quyết định điều khiển khi chưa configure.
+    out.state = NavigationState::kIdle;
+    out.velocity_source = VelocitySource::kNone;
+    out.reason = "chưa configure";
+    return out;
+  }
+
+  // State terminal chỉ sống một cycle. Về kIdle ngay đầu cycle kế tiếp để yêu cầu mới được nhận
+  // không phải chờ thêm một vòng.
+  if (isTerminal(state_))
+  {
+    enter(NavigationState::kIdle, in.now, "yêu cầu đã kết thúc", out);
+  }
+
+  // Mất pose nghĩa là không biết robot ở đâu. Khi đó controller không được chạy, và không nguồn nào
+  // được phát vận tốc — kể cả recovery. Recovery vẫn được tick để nó tự báo lỗi theo contract của
+  // nó, nhưng lệnh nó sinh ra bị chặn ở phần chốt bất biến cuối hàm.
+  const ControllerFeedback controller =
+      in.pose_available ? in.controller : ControllerFeedback::kNoValidCommand;
+
+  switch (state_)
+  {
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kIdle:
+    {
+      if (in.has_pending_request)
+      {
+        out.accept_request = true;
+        recovery_index_ = 0;
+        request_has_goal_ = in.pending_request_has_goal;
+        action_count_ = in.pending_request_action_count;
+        action_index_ = 0;
+
+        if (!request_has_goal_)
+        {
+          // D8: yêu cầu chỉ-có-action — không có gì để lập plan, vào thẳng thực thi action.
+          if (action_count_ == 0)
+          {
+            // Không goal lẫn action là vi phạm contract; mission layer đã validate nhưng lõi vẫn
+            // phải tự vệ: kết thúc tường minh thay vì treo ở một state không có đường ra.
+            finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
+                   "yêu cầu không có goal lẫn action", out);
+            break;
+          }
+          out.start_action = true;
+          out.action_index = 0;
+          action_started_at_ = in.now;
+          enter(NavigationState::kExecutingActions, in.now, "yêu cầu chỉ có action", out);
+          break;
+        }
+
+        out.start_planner = true;
+        beginPlanningCycle(in.now);
+        last_valid_control_ = in.now;
+        last_oscillation_reset_ = in.now;
+        out.reset_oscillation_origin = true;
+        enter(NavigationState::kPlanning, in.now, "nhận yêu cầu mới", out);
+      }
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kPlanning:
+    {
+      if (in.cancel_requested)
+      {
+        out.stop_planner = true;
+        enter(NavigationState::kCancelling, in.now, "huỷ khi đang lập plan", out);
+        break;
+      }
+      if (in.pause_requested)
+      {
+        out.stop_planner = true;
+        state_before_pause_ = NavigationState::kPlanning;
+        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang lập plan", out);
+        break;
+      }
+
+      if (in.planner == PlannerFeedback::kPlanReady)
+      {
+        out.apply_plan = true;
+        out.run_controller = true;  // Chạy controller ngay trong cycle này, không phí một vòng.
+        beginPlanningCycle(in.now);
+
+        // Cố ý KHÔNG làm mới last_valid_control_ và last_oscillation_reset_ ở đây. Có plan mới
+        // không chứng minh được gì về controller: nếu reset thì vòng lặp
+        // CONTROLLING -> PLANNING -> CONTROLLING sẽ làm mới đồng hồ mỗi vòng, và một controller
+        // hỏng vĩnh viễn sẽ không bao giờ chạm controller_patience. Hai đồng hồ đó chỉ được đặt lại
+        // ở ba chỗ: nhận yêu cầu mới, tiếp tục sau tạm dừng, và sau khi recovery chạy xong.
+        enter(NavigationState::kControlling, in.now, "có plan hợp lệ", out);
+        break;
+      }
+
+      if (in.planner == PlannerFeedback::kFailed)
+      {
+        ++planning_retries_;
+      }
+
+      const bool retries_exhausted = config_.max_planning_retries >= 0 &&
+                                     planning_retries_ > config_.max_planning_retries;
+      const bool patience_exhausted =
+          config_.planner_patience > 0.0 &&
+          (in.now - last_valid_plan_).toSec() > config_.planner_patience;
+
+      if (retries_exhausted || patience_exhausted)
+      {
+        out.stop_planner = true;
+        escalateToRecovery(RecoveryTrigger::kPlanningFailed, in.now,
+                           retries_exhausted ? "hết lượt lập plan" : "quá hạn lập plan", out);
+        break;
+      }
+
+      out.start_planner = true;  // Giữ planner chạy tiếp.
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kControlling:
+    {
+      if (in.cancel_requested)
+      {
+        out.stop_planner = true;
+        enter(NavigationState::kCancelling, in.now, "huỷ khi đang bám plan", out);
+        break;
+      }
+      if (in.pause_requested)
+      {
+        out.stop_planner = true;
+        state_before_pause_ = NavigationState::kControlling;
+        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang bám plan", out);
+        break;
+      }
+
+      // Plan mới tới giữa lúc đang bám plan cũ: nhận ngay, vẫn ở kControlling.
+      if (in.planner == PlannerFeedback::kPlanReady)
+      {
+        out.apply_plan = true;
+        beginPlanningCycle(in.now);
+      }
+
+      // Đi đủ xa thì không còn bị coi là quẩn — đặt lại cả đồng hồ lẫn mốc đo quãng đường.
+      if (config_.oscillation_distance > 0.0 &&
+          in.travelled_since_oscillation_reset >= config_.oscillation_distance)
+      {
+        last_oscillation_reset_ = in.now;
+        out.reset_oscillation_origin = true;
+      }
+
+      if (controller == ControllerFeedback::kGoalReached)
+      {
+        if (action_count_ > 0)
+        {
+          // D8: tới goal chưa phải là xong — mission còn action phải chạy tại chỗ. Kết quả chỉ
+          // được báo sau action cuối, để mission layer thấy trọn một chặng nav + action.
+          out.stop_planner = true;
+          out.start_action = true;
+          out.action_index = action_index_;
+          action_started_at_ = in.now;
+          enter(NavigationState::kExecutingActions, in.now, "đạt goal, còn action phải chạy", out);
+          break;
+        }
+        finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "đạt goal", out);
+        break;
+      }
+
+      if (controller == ControllerFeedback::kCommandValid)
+      {
+        last_valid_control_ = in.now;
+
+        if (config_.oscillation_timeout > 0.0 &&
+            (in.now - last_oscillation_reset_).toSec() > config_.oscillation_timeout)
+        {
+          out.stop_planner = true;
+          escalateToRecovery(RecoveryTrigger::kOscillation, in.now, "quẩn tại chỗ quá lâu", out);
+          break;
+        }
+
+        out.run_controller = true;
+        break;
+      }
+
+      // Còn lại: kIdle (cycle đầu sau khi vào state) hoặc kNoValidCommand.
+      if (config_.controller_patience > 0.0 &&
+          (in.now - last_valid_control_).toSec() > config_.controller_patience)
+      {
+        out.stop_planner = true;
+        escalateToRecovery(RecoveryTrigger::kControllingFailed, in.now,
+                           "quá hạn sinh lệnh vận tốc", out);
+        break;
+      }
+
+      if (controller == ControllerFeedback::kNoValidCommand && in.pose_available)
+      {
+        // Chưa hết kiên nhẫn: quay lại lập plan. Cố ý KHÔNG reset last_valid_control_ ở đây, nếu
+        // không thì vòng lặp lập-plan-rồi-lại-hỏng sẽ không bao giờ chạm controller_patience.
+        out.start_planner = true;
+        beginPlanningCycle(in.now);
+        enter(NavigationState::kPlanning, in.now, "controller không sinh được lệnh, lập lại plan",
+              out);
+        break;
+      }
+
+      // Mất pose thì ở nguyên kControlling (vận tốc đã bị ép về 0 ở phần chốt bất biến) cho tới khi
+      // controller_patience hết hạn. Lập lại plan không giúp được gì khi vấn đề là định vị, và
+      // nhảy sang kPlanning chỉ làm lý do vào recovery bị ghi nhận sai thành "lập plan hỏng".
+
+      out.run_controller = true;
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kRecovering:
+    {
+      if (in.cancel_requested)
+      {
+        out.cancel_recovery = true;
+        enter(NavigationState::kCancelling, in.now, "huỷ khi đang recovery", out);
+        break;
+      }
+      if (in.pause_requested)
+      {
+        // Recovery bị huỷ khi tạm dừng: giữ một behavior ở trạng thái dở dang qua một quãng dừng
+        // dài là không an toàn (nó dead-reckon theo thời gian). Resume sẽ lập plan lại từ đầu.
+        out.cancel_recovery = true;
+        state_before_pause_ = NavigationState::kPlanning;
+        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang recovery", out);
+        break;
+      }
+
+      if (in.recovery == RecoveryFeedback::kSucceeded || in.recovery == RecoveryFeedback::kFailed)
+      {
+        // Behavior chạy xong (thành công hay không) thì thử lập plan lại. Lỗi kế tiếp sẽ dùng
+        // behavior kế tiếp; hết behavior thì ABORTED.
+        ++recovery_index_;
+        out.start_planner = true;
+        beginPlanningCycle(in.now);
+        last_valid_control_ = in.now;
+        enter(NavigationState::kPlanning, in.now,
+              in.recovery == RecoveryFeedback::kSucceeded ? "recovery xong, lập plan lại"
+                                                          : "recovery thất bại, lập plan lại",
+              out);
+        break;
+      }
+
+      out.tick_recovery = true;
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kExecutingActions:
+    {
+      if (in.cancel_requested)
+      {
+        out.cancel_action = true;
+        enter(NavigationState::kCancelling, in.now, "huỷ khi đang chạy action", out);
+        break;
+      }
+      if (in.pause_requested)
+      {
+        // Khác recovery, action KHÔNG bị huỷ khi tạm dừng: robot đang đứng yên nên không có rủi ro
+        // dead-reckon, còn chạy lại một action thiết bị (nâng/hạ, sạc) từ đầu thì không chắc an
+        // toàn — action không idempotent. Tạm dừng chỉ ngừng tick; resume tick tiếp đúng action đó.
+        state_before_pause_ = NavigationState::kExecutingActions;
+        enter(NavigationState::kPaused, in.now, "tạm dừng khi đang chạy action", out);
+        break;
+      }
+
+      if (in.action == ActionFeedback::kFailed)
+      {
+        // Action hỏng không có đường recovery: recovery behavior là công cụ phục hồi NAVIGATION
+        // (dọn costmap, lùi, xoay), không giúp gì được một thiết bị đang hỏng. Kết thúc tường minh
+        // để mission layer quyết định làm gì với phần còn lại của order.
+        finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now, "action thất bại",
+               out);
+        break;
+      }
+
+      if (in.action == ActionFeedback::kSucceeded)
+      {
+        ++action_index_;
+        if (action_index_ < action_count_)
+        {
+          out.start_action = true;
+          out.action_index = action_index_;
+          action_started_at_ = in.now;  // Trần thời gian tính cho TỪNG action, không cho cả chuỗi.
+          break;  // Vẫn ở kExecutingActions, chuyển sang action kế tiếp.
+        }
+        finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now,
+               "action cuối đã xong", out);
+        break;
+      }
+
+      // Lưới an toàn cuối cùng cho action treo (handler hỏng, thiết bị câm lặng vĩnh viễn).
+      // Cơ chế timeout CHÍNH là của từng ActionHandler; lưới này mặc định tắt.
+      if (config_.action_patience > 0.0 &&
+          (in.now - action_started_at_).toSec() > config_.action_patience)
+      {
+        out.cancel_action = true;  // Bảo port dừng thiết bị an toàn trước khi kết thúc chặng.
+        finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now,
+               "action quá hạn action_patience", out);
+        break;
+      }
+
+      // Mất pose KHÔNG chặn tick action: robot đứng yên, thao tác thiết bị không cần định vị.
+      // Vận tốc vẫn bị ép về 0 ở phần chốt bất biến cuối hàm như mọi state phải dừng khác.
+      out.tick_action = true;
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kPaused:
+    {
+      if (in.cancel_requested)
+      {
+        enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out);
+        break;
+      }
+      if (in.resume_requested)
+      {
+        // Đặt lại toàn bộ đồng hồ kiên nhẫn: một lần tạm dừng dài không được tính là "planner chậm"
+        // hay "controller hỏng", nếu không thì resume xong là rơi thẳng vào recovery.
+        beginPlanningCycle(in.now);
+        last_valid_control_ = in.now;
+        last_oscillation_reset_ = in.now;
+        out.reset_oscillation_origin = true;
+
+        if (state_before_pause_ == NavigationState::kControlling)
+        {
+          out.run_controller = true;
+          enter(NavigationState::kControlling, in.now, "tiếp tục bám plan", out);
+        }
+        else if (state_before_pause_ == NavigationState::kExecutingActions)
+        {
+          // Action không bị huỷ khi tạm dừng nên không start lại — tick tiếp đúng action dở dang.
+          // Mốc action_patience được gieo lại: một quãng dừng dài không được tính vào trần thời
+          // gian của action, nếu không resume xong là ABORTED oan ngay lập tức.
+          action_started_at_ = in.now;
+          out.tick_action = true;
+          enter(NavigationState::kExecutingActions, in.now, "tiếp tục chạy action", out);
+        }
+        else
+        {
+          out.start_planner = true;
+          enter(NavigationState::kPlanning, in.now, "tiếp tục lập plan", out);
+        }
+      }
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kCancelling:
+    {
+      // Nguồn vận tốc đã là kNone nên bộ trọng tài đang giảm tốc về 0 theo trần gia tốc; trạng thái
+      // này vì vậy luôn kết thúc sau hữu hạn cycle, không cần thêm timeout.
+      if (in.robot_stopped)
+      {
+        finish(NavigationState::kCancelled, NavigationOutcome::kCancelled, in.now,
+               "robot đã dừng hẳn", out);
+      }
+      break;
+    }
+
+    // --------------------------------------------------------------------------------------
+    case NavigationState::kSucceeded:
+    case NavigationState::kAborted:
+    case NavigationState::kCancelled:
+      // Không tới được: đã chuyển về kIdle ở đầu hàm.
+      break;
+  }
+
+  // ------------------------------------------------------------------------------------------
+  // Chốt bất biến trước khi trả ra. Ba dòng này là hàng rào an toàn cuối cùng của lõi.
+  // ------------------------------------------------------------------------------------------
+  out.state = state_;
+
+  if (state_ == NavigationState::kControlling)
+  {
+    out.velocity_source = VelocitySource::kController;
+  }
+  else if (state_ == NavigationState::kRecovering)
+  {
+    // Chỉ behavior thật sự lái robot mới được cấp quyền phát vận tốc. Behavior one-shot (đợi, xoá
+    // costmap) giữ nguồn ở kNone, nên arbiter không phải đổi nguồn hai lần cho một lượt không có
+    // vận tốc nào — mỗi lần đổi nguồn tốn một cycle zero (mục 1.6).
+    out.velocity_source = in.active_recovery_output == RecoveryOutputKind::kVelocity
+                              ? VelocitySource::kRecovery
+                              : VelocitySource::kNone;
+  }
+  else
+  {
+    out.velocity_source = VelocitySource::kNone;
+  }
+
+  if (!in.pose_available)
+  {
+    out.velocity_source = VelocitySource::kNone;
+    out.run_controller = false;
+  }
+
+  if (mustBeStopped(state_))
+  {
+    out.velocity_source = VelocitySource::kNone;
+    out.run_controller = false;
+    out.tick_recovery = false;
+  }
+
+  return out;
+}
+
+}  // namespace move_base2

src/velocity_arbiter.cpp

@@ -0,0 +1,245 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — cài đặt bộ trọng tài vận tốc.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <move_base2/core/velocity_arbiter.h>
+
+#include <algorithm>
+#include <cmath>
+#include <sstream>
+
+namespace move_base2
+{
+namespace
+{
+
+/// @brief Giá trị hữu hạn (không NaN, không Inf).
+bool isFinite(double value)
+{
+  return std::isfinite(value);
+}
+
+/// @brief Đưa @p value về [lower, upper]. Trả true qua @p clamped nếu có cắt.
+double clampTo(double value, double lower, double upper, bool& clamped)
+{
+  if (value < lower)
+  {
+    clamped = true;
+    return lower;
+  }
+  if (value > upper)
+  {
+    clamped = true;
+    return upper;
+  }
+  return value;
+}
+
+}  // namespace
+
+// ================================================================================================
+// VelocityLimits
+// ================================================================================================
+
+bool VelocityLimits::validate(std::string& error) const
+{
+  if (!(max_vel_x > 0.0))
+  {
+    error = "max_vel_x phải > 0 [m/s]";
+    return false;
+  }
+  if (min_vel_x > 0.0)
+  {
+    error = "min_vel_x là trần tốc độ LÙI nên phải <= 0 [m/s]; đặt 0 nếu cấm lùi";
+    return false;
+  }
+  if (!(max_vel_theta > 0.0))
+  {
+    error = "max_vel_theta phải > 0 [rad/s]";
+    return false;
+  }
+  if (!(max_accel_x > 0.0))
+  {
+    error = "max_accel_x phải > 0 [m/s^2]";
+    return false;
+  }
+  if (!(max_accel_theta > 0.0))
+  {
+    error = "max_accel_theta phải > 0 [rad/s^2]";
+    return false;
+  }
+  if (zero_velocity_epsilon < 0.0)
+  {
+    error = "zero_velocity_epsilon phải >= 0";
+    return false;
+  }
+  return true;
+}
+
+std::string VelocityLimits::describe() const
+{
+  std::ostringstream out;
+  out << "VelocityLimits:\n";
+  out << "  max_vel_x            : " << max_vel_x << " m/s (tiến)\n";
+  out << "  min_vel_x            : " << min_vel_x << " m/s (lùi"
+      << (min_vel_x == 0.0 ? ", đang cấm lùi" : "") << ")\n";
+  out << "  max_vel_theta        : " << max_vel_theta << " rad/s\n";
+  out << "  max_accel_x          : " << max_accel_x << " m/s^2\n";
+  out << "  max_accel_theta      : " << max_accel_theta << " rad/s^2\n";
+  out << "  zero_velocity_epsilon: " << zero_velocity_epsilon << '\n';
+  return out.str();
+}
+
+// ================================================================================================
+// VelocityArbiter
+// ================================================================================================
+
+robot_geometry_msgs::Twist VelocityArbiter::zeroTwist()
+{
+  return robot_geometry_msgs::Twist();
+}
+
+bool VelocityArbiter::configure(const VelocityLimits& limits, std::string& error)
+{
+  if (!limits.validate(error))
+  {
+    initialized_ = false;
+    return false;
+  }
+
+  limits_ = limits;
+  initialized_ = true;
+  reset();
+  return true;
+}
+
+void VelocityArbiter::reset()
+{
+  active_source_ = VelocitySource::kNone;
+  last_command_ = zeroTwist();
+  non_finite_rejections_ = 0;
+  velocity_clamps_ = 0;
+  acceleration_clamps_ = 0;
+  handover_cycles_ = 0;
+}
+
+bool VelocityArbiter::stopped() const
+{
+  return std::abs(last_command_.linear.x) <= limits_.zero_velocity_epsilon &&
+         std::abs(last_command_.linear.y) <= limits_.zero_velocity_epsilon &&
+         std::abs(last_command_.angular.z) <= limits_.zero_velocity_epsilon;
+}
+
+robot_geometry_msgs::Twist VelocityArbiter::sanitize(const robot_geometry_msgs::Twist& candidate)
+{
+  robot_geometry_msgs::Twist clean;
+
+  // NaN/Inf từ bất kỳ trục nào làm hỏng cả lệnh: không có cách nào "sửa một phần" một lệnh mà bộ
+  // sinh ra nó đang ở trạng thái hỏng. Trả 0 và đếm lại để tầng trên phát hiện được.
+  if (!isFinite(candidate.linear.x) || !isFinite(candidate.linear.y) ||
+      !isFinite(candidate.linear.z) || !isFinite(candidate.angular.x) ||
+      !isFinite(candidate.angular.y) || !isFinite(candidate.angular.z))
+  {
+    ++non_finite_rejections_;
+    return clean;
+  }
+
+  bool clamped = false;
+  clean.linear.x = clampTo(candidate.linear.x, limits_.min_vel_x, limits_.max_vel_x, clamped);
+  clean.angular.z =
+      clampTo(candidate.angular.z, -limits_.max_vel_theta, limits_.max_vel_theta, clamped);
+
+  // Robot của workspace là phi holonomic ở mức contract cmd_vel: mọi thành phần còn lại bị bỏ, cố
+  // ý không truyền tiếp để tránh một plugin lạ đẩy ra trục mà tầng dưới không kiểm.
+  clean.linear.y = 0.0;
+  clean.linear.z = 0.0;
+  clean.angular.x = 0.0;
+  clean.angular.y = 0.0;
+
+  if (clamped)
+  {
+    ++velocity_clamps_;
+  }
+  return clean;
+}
+
+robot_geometry_msgs::Twist VelocityArbiter::limitAcceleration(
+    const robot_geometry_msgs::Twist& target, double dt)
+{
+  if (dt <= 0.0)
+  {
+    // Không biết dt thật thì không có cơ sở nào để giới hạn gia tốc. Trả nguyên lệnh đã sanitize
+    // thay vì bịa ra một chu kỳ danh nghĩa — bịa chính là lớp lỗi mà thiết kế này muốn tránh.
+    return target;
+  }
+
+  robot_geometry_msgs::Twist limited = target;
+  bool clamped = false;
+
+  const double max_dx = limits_.max_accel_x * dt;
+  const double max_dtheta = limits_.max_accel_theta * dt;
+
+  limited.linear.x = clampTo(target.linear.x, last_command_.linear.x - max_dx,
+                             last_command_.linear.x + max_dx, clamped);
+  limited.angular.z = clampTo(target.angular.z, last_command_.angular.z - max_dtheta,
+                              last_command_.angular.z + max_dtheta, clamped);
+
+  if (clamped)
+  {
+    ++acceleration_clamps_;
+  }
+  return limited;
+}
+
+robot_geometry_msgs::Twist VelocityArbiter::arbitrate(VelocitySource source,
+                                                      const robot_geometry_msgs::Twist& candidate,
+                                                      double dt)
+{
+  if (!initialized_)
+  {
+    // Guard bắt buộc: chưa configure thì không phát gì cả.
+    last_command_ = zeroTwist();
+    active_source_ = VelocitySource::kNone;
+    return last_command_;
+  }
+
+  if (source == VelocitySource::kNone)
+  {
+    // Lệnh 0 TỨC THÌ, không giảm tốc dần. Ba lý do, theo thứ tự quan trọng:
+    //  1. Lệnh vận tốc là giá trị được chốt lại ở tầng dưới. Nếu control loop dừng giữa lúc đang
+    //     giảm tốc dần thì lệnh khác 0 cuối cùng còn nguyên hiệu lực và robot chạy tiếp.
+    //  2. Bất biến "state phải dừng thì lệnh đúng bằng 0" trở thành kiểm được, không phải "gần 0".
+    //  3. Giảm tốc theo động học là việc của bộ điều khiển bánh xe, nơi biết tải và ma sát thật.
+    active_source_ = VelocitySource::kNone;
+    last_command_ = zeroTwist();
+    return last_command_;
+  }
+
+  // Đổi nguồn: chèn đúng một cycle vận tốc 0 trước khi nguồn mới được phát. Hai bộ sinh lệnh giữ
+  // trạng thái gia tốc riêng nên nối thẳng chúng lại gây giật. Cycle 0 này cũng là ranh giới rõ
+  // ràng để soi log khi điều tra sự cố.
+  if (active_source_ != VelocitySource::kNone && active_source_ != source)
+  {
+    ++handover_cycles_;
+    active_source_ = source;
+    last_command_ = zeroTwist();
+    return last_command_;
+  }
+
+  active_source_ = source;
+  last_command_ = limitAcceleration(sanitize(candidate), dt);
+  return last_command_;
+}
+
+robot_geometry_msgs::Twist VelocityArbiter::emergencyStop()
+{
+  active_source_ = VelocitySource::kNone;
+  last_command_ = zeroTwist();
+  return last_command_;
+}
+
+}  // namespace move_base2