optimal

include/move_base2/control_loop.h

@@ -147,6 +147,12 @@ public:
    */
   bool step();
 
+  /// @brief Plan đang được bám. Rỗng khi chưa có plan nào — bên gọi phải tự kiểm.
+  const std::vector<robot_geometry_msgs::PoseStamped>& latestPlan() const
+  {
+    return latest_plan_;
+  }
+
   /// @brief Lệnh vận tốc phát ra ở cycle gần nhất.
   const robot_geometry_msgs::Twist& lastCommand() const
   {
@@ -172,6 +178,12 @@ public:
     return state_machine_.state();
   }
 
+  /// @brief Đang có yêu cầu chạy dở (kể cả đang pause) — false ngay khi kết quả được báo.
+  bool hasActiveRequest() const
+  {
+    return has_active_request_;
+  }
+
   /// @brief Kết quả của yêu cầu vừa kết thúc; chuỗi rỗng nếu chưa có yêu cầu nào kết thúc.
   const char* lastOutcome() const;
 

include/move_base2/core/state_machine.h

@@ -297,6 +297,26 @@ private:
                           StateMachineOutput& out);
 
   /// @brief Vào state terminal và bật cờ báo kết quả đúng một lần.
+  /**
+   * @brief Nhận yêu cầu đang chờ và đưa máy vào state chạy tương ứng.
+   *
+   * Tách ra vì có HAI đường vào: từ @c kIdle (bình thường) và từ mọi state đang chạy khi có yêu cầu
+   * mới chen ngang (preempt).
+   */
+  void acceptPendingRequest(const StateMachineInput& in, StateMachineOutput& out);
+
+  /**
+   * @brief Yêu cầu mới chen ngang yêu cầu đang chạy.
+   *
+   * @return true nếu đã preempt — bên gọi phải `break` ngay, mọi xử lý còn lại của state cũ không
+   *         còn nghĩa.
+   *
+   * Goal mới phải thay goal cũ **ngay**, không xếp hàng: người vận hành bấm goal mới nghĩa là goal
+   * cũ không còn muốn nữa, và chờ tới khi robot đi hết chặng cũ là hành vi không ai mong đợi. Bản
+   * cũ preempt ngay qua action server; mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2).
+   */
+  bool preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out);
+
   void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now,
               const char* reason, StateMachineOutput& out);
 

include/move_base2/io/costmap_exporter.h

@@ -0,0 +1,91 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — kết xuất costmap ra dạng OccupancyGrid cho host publish lên rviz.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_IO_COSTMAP_EXPORTER_H_
+#define MOVE_BASE2_IO_COSTMAP_EXPORTER_H_
+
+#include <mutex>
+#include <string>
+
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+
+namespace robot_costmap_2d
+{
+class Costmap2DROBOT;
+}
+
+namespace move_base2
+{
+
+/**
+ * @class CostmapExporter
+ * @brief Chuyển lưới chi phí sang `OccupancyGrid` (bản đầy đủ) hoặc `OccupancyGridUpdate` (phần
+ *        thay đổi), đúng giao thức mà host đang dùng để publish lên rviz.
+ *
+ * ## Vì sao LUÔN gửi lưới đầy đủ
+ *
+ * Giao thức `OccupancyGridUpdate` (chỉ gửi phần thay đổi) chỉ đúng khi có **đúng một** consumer.
+ * Host lại gọi `getGlobalData()` từ HAI ros::Timer — costmap (1 Hz) và plan (10 Hz) — nên timer plan
+ * tiêu thụ hầu hết các lần gọi rồi vứt phần costmap đi. Tới lượt timer costmap, vùng thay đổi đã bị
+ * reset, và lưới cache chỉ được dựng lại khi hình học đổi: local costmap là rolling window nên vẫn
+ * tươi, còn **global costmap đóng băng ở ảnh chụp đầu tiên vĩnh viễn**. Đã xảy ra.
+ *
+ * Bản cũ có cờ `always_send_full_costmap` cho đúng tình huống này. Ở đây chọn luôn gửi đầy đủ:
+ * 400×400 ô là 160 KB, ở 1–5 Hz là cái giá rẻ để đổi lấy việc không còn một lớp lỗi im lặng.
+ *
+ * ## Bảng quy đổi chi phí
+ *
+ * `costmap_2d` dùng thang 0–255 với vài giá trị đặc biệt; `OccupancyGrid` dùng −1 và 0–100. Bảng
+ * quy đổi giữ nguyên của bản cũ, kể cả hai giá trị riêng của workspace này (`PREFERRED_SPACE` →
+ * 120, `CRITICAL_SPACE` → 150) vốn nằm **ngoài** dải chuẩn 0–100 — rviz hiển thị chúng như ô lạ,
+ * nhưng đó là quy ước đang dùng và đổi nó là đổi thứ người vận hành đã quen nhìn.
+ *
+ * @note Thread-safe. Host gọi từ nhiều ros::Timer khác nhau (costmap và plan có timer riêng nhưng
+ *       cùng gọi `getGlobalData`), nên trạng thái nội bộ phải được bảo vệ.
+ */
+class CostmapExporter
+{
+public:
+  CostmapExporter();
+
+  /**
+   * @brief Gắn costmap nguồn.
+   * @param costmap      **Non-owning**, được phép null (chưa dựng costmap thì kết xuất rỗng).
+   * @param global_frame Frame ghi vào header của lưới xuất ra.
+   */
+  void attach(robot_costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame);
+
+  /**
+   * @brief Điền lưới đầy đủ.
+   *
+   * @param[out] grid       Lưới đầy đủ tại thời điểm gọi.
+   * @param[out] update     Không dùng — giữ trong chữ ký vì contract host có trường này.
+   * @param[out] is_updated Luôn false: đây luôn là bản đầy đủ. Host dùng cờ này để chọn topic.
+   *
+   * Không làm gì khi chưa gắn costmap hoặc costmap còn kích thước 0 — @p grid giữ nguyên, để host
+   * không publish một lưới rỗng đè lên lưới đang hiển thị.
+   */
+  void fill(robot_nav_msgs::OccupancyGrid& grid, robot_map_msgs::OccupancyGridUpdate& update,
+            bool& is_updated);
+
+private:
+  void prepareGridLocked();
+
+  mutable std::mutex mutex_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
+  std::string global_frame_ = "map";
+
+  robot_nav_msgs::OccupancyGrid grid_;
+  double saved_origin_x_ = 0.0;  ///< [m]
+  double saved_origin_y_ = 0.0;  ///< [m]
+};
+
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_IO_COSTMAP_EXPORTER_H_

include/move_base2/navigation_runtime.h

@@ -0,0 +1,229 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * move_base2 — dựng toàn bộ thành phần runtime thật từ config và TF.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MOVE_BASE2_NAVIGATION_RUNTIME_H_
+#define MOVE_BASE2_NAVIGATION_RUNTIME_H_
+
+#include <memory>
+#include <string>
+
+#include <robot_costmap_2d/costmap_2d_robot.h>
+
+#include <move_base2/bridges/mission_adapter_bridge.h>
+#include <move_base2/config/move_base2_config.h>
+#include <move_base2/control_loop.h>
+#include <move_base2/io/costmap_exporter.h>
+#include <move_base2/ports/clock_port.h>
+#include <move_base2/ports/pose_port.h>
+#include <move_base2/runners/action_runner.h>
+#include <move_base2/runners/controller_runner.h>
+#include <move_base2/runners/planner_runner.h>
+#include <move_base2/runners/recovery_runner.h>
+
+namespace tf3
+{
+class BufferCore;
+}
+
+namespace move_base2
+{
+
+/**
+ * @class CostmapPosePort
+ * @brief Cổng pose lấy từ costmap.
+ *
+ * `Costmap2DROBOT::getRobotPose` đã gói sẵn việc tra TF kèm `transform_tolerance` và trả false khi
+ * transform quá cũ — đúng ngữ nghĩa mà @ref PosePort cần, nên không có gì phải viết lại.
+ *
+ * @note Con trỏ costmap là **non-owning**. Lấy lại mỗi lần dùng chứ không cache `Costmap2D*` bên
+ *       trong: con trỏ costmap có thể bị thay.
+ */
+class CostmapPosePort final : public PosePort
+{
+public:
+  explicit CostmapPosePort(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr) : costmap_(costmap)
+  {
+  }
+
+  void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
+  {
+    costmap_ = costmap;
+  }
+
+  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
+  {
+    if (costmap_ == nullptr)
+    {
+      return false;
+    }
+    return costmap_->getRobotPose(pose);
+  }
+
+private:
+  robot_costmap_2d::Costmap2DROBOT* costmap_;
+};
+
+/**
+ * @class NavigationRuntime
+ * @brief Sở hữu và dựng mọi thành phần thật của runtime: costmap, runner, bridge.
+ *
+ * Tách khỏi @ref NavigationServer để chỗ **lắp ráp** không lẫn với chỗ **hiện thực contract host**.
+ * `NavigationServer` vẫn nhận cổng qua `configureLoop`, nên nó kiểm được bằng cổng giả y như trước;
+ * lớp này chỉ là một cách dựng bộ cổng đó từ config thật.
+ *
+ * ## Thứ tự dựng là bắt buộc, không phải tuỳ ý
+ *
+ * `state_machine.recovery_behavior_count` **không đến từ YAML** — nó là số behavior
+ * `RecoveryRunner` nạp được **thật**. Đọc từ config thì một behavior hỏng vẫn khiến state machine
+ * tin là còn đường phục hồi, và lỗi đầu tiên sẽ dẫn thẳng tới `ABORTED` mà không ai hiểu vì sao.
+ * Trình tự bắt buộc, được @ref build tuân thủ:
+ *
+ *   `MoveBase2Config::load()` → `RecoveryRunner::configure()` → gán `recovery_behavior_count`
+ *   → `MoveBase2Config::validate()`
+ *
+ * ## Costmap
+ *
+ * Hai costmap được dựng ở đây và **chỉ `start()` sau khi mọi thứ khác đã sẵn sàng**: `start()` cho
+ * chạy thread cập nhật của costmap, và một thread cập nhật chạy trong khi planner chưa nạp xong là
+ * cửa sổ để mọi thứ chạm vào nhau ở trạng thái nửa vời.
+ */
+class NavigationRuntime
+{
+public:
+  NavigationRuntime();
+  ~NavigationRuntime();
+
+  NavigationRuntime(const NavigationRuntime&) = delete;
+  NavigationRuntime& operator=(const NavigationRuntime&) = delete;
+
+  /**
+   * @brief Pha 1: đọc config và dựng hai costmap. **Chưa nạp plugin nào.**
+   *
+   * Tách khỏi @ref buildRunners là bắt buộc, không phải để cho gọn. `SBPLLatticePlanner::initialize`
+   * **chặn** cho tới khi costmap có kích thước khác 0 (`sbpl_lattice_planner.cpp:132`), mà kích
+   * thước đó chỉ được đặt khi static map tới `StaticLayer`. Nếu nạp planner ngay trong một pha thì
+   * planner chờ map, còn map thì chờ pha đó xong để được đẩy vào — khoá chết, và SBPL `exit(1)` sau
+   * 2 giây, giết cả tiến trình.
+   *
+   * Trình tự đúng, giống hệt bản cũ (`move_base.cpp:260` dựng costmap → `:287` đẩy map → `:306` nạp
+   * planner): dựng costmap → gắn vào SensorGateway và phát lại static map → rồi mới nạp plugin.
+   *
+   * @param tf         Buffer TF do host cấp. Bắt buộc khác null; phải sống lâu hơn object này.
+   * @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false.
+   */
+  bool buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf, std::string& error);
+
+  /**
+   * @brief Pha 2: nạp planner, controller, recovery, action rồi kiểm cấu hình.
+   *
+   * Chỉ gọi sau khi static map đã được đẩy vào costmap — xem @ref buildCostmaps.
+   *
+   * Thất bại ở bất kỳ bước nào cũng để runtime ở trạng thái **chưa dựng** (@ref built false), chứ
+   * không phải nửa dựng nửa không.
+   */
+  bool buildRunners(std::string& error);
+
+  /// @brief Hai costmap đã dựng xong chưa (pha 1).
+  bool costmapsReady() const
+  {
+    return global_costmap_ != nullptr && local_costmap_ != nullptr;
+  }
+
+  bool built() const
+  {
+    return built_;
+  }
+
+  /// @brief Cho costmap bắt đầu cập nhật. Gọi sau khi control loop đã được cấu hình xong.
+  void start();
+
+  /// @brief Dừng cập nhật costmap.
+  void stop();
+
+  /// @brief Bộ cổng để bơm vào @ref ControlLoop. Rỗng nếu chưa @ref build.
+  ControlLoopDeps deps();
+
+  const MoveBase2Config& config() const
+  {
+    return config_;
+  }
+
+  robot_costmap_2d::Costmap2DROBOT* globalCostmap()
+  {
+    return global_costmap_.get();
+  }
+
+  robot_costmap_2d::Costmap2DROBOT* localCostmap()
+  {
+    return local_costmap_.get();
+  }
+
+  MissionAdapterBridge& mission()
+  {
+    return mission_;
+  }
+
+  PlannerRunner& planner()
+  {
+    return planner_;
+  }
+
+  ControllerRunner& controller()
+  {
+    return controller_;
+  }
+
+  /// @brief Bộ kết xuất costmap cho host publish lên rviz.
+  CostmapExporter& globalExporter()
+  {
+    return global_exporter_;
+  }
+
+  CostmapExporter& localExporter()
+  {
+    return local_exporter_;
+  }
+
+private:
+  MoveBase2Config config_;
+  bool built_ = false;
+
+  std::shared_ptr<tf3::BufferCore> tf_;
+
+  // Costmap phải được khai TRƯỚC các runner: runner giữ con trỏ tới chúng, nên chúng phải bị huỷ
+  // SAU. Thứ tự khai báo thành viên chính là thứ tự huỷ ngược.
+  std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> global_costmap_;
+  std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> local_costmap_;
+
+  SystemClock clock_;
+
+  /**
+   * HAI nguồn pose, khác FRAME — không gộp được.
+   *
+   * `Costmap2DROBOT::getRobotPose` trả pose trong **global frame của chính costmap đó**: costmap
+   * global dùng `map`, costmap local dùng `odom`. Global planner lập plan trên lưới `map` nên start
+   * phải ở `map`; local planner và recovery làm việc trên lưới `odom` nên pose phải ở `odom`.
+   *
+   * Dùng nhầm nguồn không gây lỗi nào nhìn thấy được — chỉ là start của planner nằm ở gốc odom thay
+   * vì vị trí thật, và planner báo "failed to set start state". Đã xảy ra một lần.
+   */
+  CostmapPosePort global_pose_;  ///< frame `map` — planner, chống quẩn, pose báo về host.
+  CostmapPosePort local_pose_;   ///< frame `odom` — controller và recovery.
+  CostmapExporter global_exporter_;
+  CostmapExporter local_exporter_;
+
+  PlannerRunner planner_;
+  ControllerRunner controller_;
+  RecoveryRunner recovery_;
+  ActionRunner action_;
+  MissionAdapterBridge mission_;
+};
+
+}  // namespace move_base2
+
+#endif  // MOVE_BASE2_NAVIGATION_RUNTIME_H_

include/move_base2/navigation_server.h

@@ -9,9 +9,11 @@
 #ifndef MOVE_BASE2_NAVIGATION_SERVER_H_
 #define MOVE_BASE2_NAVIGATION_SERVER_H_
 
+#include <atomic>
 #include <map>
 #include <memory>
 #include <mutex>
+#include <thread>
 #include <string>
 #include <vector>
 
@@ -20,6 +22,7 @@
 #include <move_base2/control_loop.h>
 #include <move_base2/core/navigation_request.h>
 #include <move_base2/io/sensor_gateway.h>
+#include <move_base2/navigation_runtime.h>
 
 namespace move_base2
 {
@@ -59,9 +62,36 @@ public:
   bool configureLoop(const ControlLoopConfig& config, const ControlLoopDeps& deps,
                      std::string& error);
 
-  /// @brief Chạy một control cycle. @return false khi yêu cầu hiện tại vừa kết thúc.
+  /**
+   * @brief Chạy một control cycle. @return false khi yêu cầu hiện tại vừa kết thúc.
+   *
+   * Ở runtime thật, hàm này do control thread nội bộ gọi (@ref startControlThread). Phơi ra public
+   * để test tự lái từng cycle mà không cần thread.
+   */
   bool spinOnce();
 
+  /**
+   * @brief Khởi động control thread — vòng lặp gọi @ref spinOnce ở @p frequency.
+   *
+   * Contract `BaseNavigation` **không có hàm spin nào**: host nạp plugin, gọi `initialize()`, rồi
+   * chỉ tương tác qua `moveTo`/`getTwist`/`getFeedback`. Bản `move_base` cũ tự lái mình bằng thread
+   * planner riêng cộng action server; runtime này vì thế cũng phải tự lái. Không có thread này thì
+   * goal được xếp vào chỗ chờ rồi nằm im vĩnh viễn — không cycle nào chạy, không state nào đổi,
+   * không lệnh nào phát, và **không log gì cả**.
+   *
+   * @param frequency [Hz] Nhịp control loop. <= 0 bị từ chối.
+   * @return false nếu chưa configure, thread đã chạy, hoặc nhịp không hợp lệ.
+   */
+  bool startControlThread(double frequency);
+
+  /// @brief Dừng và join control thread. An toàn khi gọi nhiều lần.
+  void stopControlThread();
+
+  bool controlThreadRunning() const
+  {
+    return control_thread_running_.load();
+  }
+
   const ControlLoop& loop() const
   {
     return loop_;
@@ -115,6 +145,15 @@ public:
 
   void initialize(robot::TFListenerPtr tf) override;
 
+  /**
+   * @brief Dừng control thread và cập nhật costmap — quiesce trước khi host thả object.
+   *
+   * Cho host một điểm dừng chủ động TRONG trình tự tắt của nó, khi tiến trình còn sống đầy đủ;
+   * phần join các thread còn lại (planner, map update) nằm ở destructor. Sau lời gọi này object
+   * không phát lệnh nữa nhưng các getter (getFeedback, getTwist...) vẫn gọi được. Idempotent.
+   */
+  void shutdown() override;
+
   void setRobotFootprint(const std::vector<robot_geometry_msgs::Point>& fprt) override;
   std::vector<robot_geometry_msgs::Point> getRobotFootprint() override;
 
@@ -204,11 +243,50 @@ private:
    */
   void pushHostInputsToController();
 
+  /**
+   * @brief Chuyển các yêu cầu pause/resume/cancel mà host đã đặt xuống lõi.
+   *
+   * Gọi từ @ref spinOnce, tức control thread. Host gọi `pause`/`resume`/`cancel` từ thread OPC-UA
+   * và VDA5050; `ControlLoop` không thread-safe nên chúng chỉ được ghi nhận, không chuyển tiếp ngay.
+   */
+  void drainLifecycleRequests();
+
+  /**
+   * @brief Điền plan và footprint vào dữ liệu xuất cho host.
+   *
+   * @param exporter Nguồn lưới costmap; null thì chỉ điền plan và footprint (chế độ test, chưa có
+   *                 runtime thật).
+   */
+  /**
+   * @brief Chụp plan toàn cục và quỹ đạo cục bộ vào bộ đệm. Chạy trên **control thread**.
+   *
+   * Lấy quỹ đạo cục bộ nghĩa là gọi vào plugin controller; plugin không thread-safe nên việc đó chỉ
+   * được làm ở đây, không phải trong getter mà host gọi từ ros::Timer.
+   */
+  void cachePlans();
+
+  /**
+   * @brief Dựng dữ liệu hiển thị cho host, trả về **theo giá trị**.
+   *
+   * @warning Tuyệt đối không ghi vào `global_data_`/`local_data_`: host gọi hai getter này từ BỐN
+   *          ros::Timer khác nhau, hai trong số đó dùng chung một thành viên. Ghi đồng thời đã gây
+   *          `std::bad_alloc` rồi hỏng heap và giết tiến trình.
+   */
+  robot::move_base_core::PlannerDataOutput buildPlannerData(CostmapExporter* exporter,
+                                                            bool local_plan);
+
   /// @brief Ánh xạ state của lõi sang enum trạng thái của contract host.
   static robot::move_base_core::State toHostState(NavigationState state);
 
   ControlLoop loop_;
   SensorGateway sensors_;
+
+  /// Thành phần runtime thật, dựng trong @ref initialize. Null khi cổng được bơm từ ngoài (test).
+  std::unique_ptr<NavigationRuntime> runtime_;
+
+  /// Control thread — thread DUY NHẤT chạy control loop và phát cmd_vel.
+  std::thread control_thread_;
+  std::atomic<bool> control_thread_running_{ false };
   robot::TFListenerPtr tf_;
 
   /// Bảo vệ dữ liệu sensor và footprint: host ghi từ thread của nó, control loop đọc.
@@ -220,6 +298,21 @@ private:
   /// Frame đóng dấu lên lệnh vận tốc gửi host. Chép từ config lúc @ref configureLoop.
   std::string robot_base_frame_ = "base_link";
 
+  /**
+   * Thời điểm cycle gần nhất còn yêu cầu đang chạy — mốc cho cửa ân hạn của @ref publishCommand.
+   * Zero = chưa từng điều khiển, stamp của twist đứng yên từ đầu (host không publish /cmd_vel,
+   * teleop dùng được ngay từ lúc boot).
+   */
+  robot::Time last_command_active_time_;
+
+  /// Frame của plan toàn cục và của quỹ đạo cục bộ — chỉ dùng khi chính plan không mang frame.
+  std::string global_frame_ = "map";
+  std::string local_frame_ = "odom";
+
+  /// Bộ đệm plan do control thread ghi, host đọc. Bảo vệ bởi @c data_mutex_.
+  robot_nav_2d_msgs::Path2D cached_global_plan_;
+  robot_nav_2d_msgs::Path2D cached_local_plan_;
+
   /**
    * Trần vận tốc host vừa đặt, chờ được đẩy xuống controller ở cycle kế tiếp.
    *
@@ -233,6 +326,11 @@ private:
   bool has_pending_linear_backward_ = false;
   bool has_pending_angular_ = false;
 
+  /// Yêu cầu vòng đời host đã đặt, chờ chuyển xuống lõi ở cycle kế tiếp.
+  bool pause_requested_ = false;
+  bool resume_requested_ = false;
+  bool cancel_requested_ = false;
+
   std::string last_reject_reason_;
 };
 

include/move_base2/ports/controller_port.h

@@ -15,6 +15,7 @@
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/Twist.h>
 #include <robot_geometry_msgs/Vector3.h>
+#include <robot_nav_2d_msgs/Path2D.h>
 
 namespace move_base2
 {
@@ -61,6 +62,17 @@ public:
   /// @brief Đã tới đích theo sai số đã đặt hay chưa.
   virtual bool isGoalReached() = 0;
 
+  /**
+   * @brief Quỹ đạo CỤC BỘ mà controller vừa sinh ra.
+   *
+   * Khác plan toàn cục: đây là đoạn đường ngắn controller thật sự đang bám, và là thứ người vận
+   * hành nhìn trên rviz để biết robot "đang định đi thế nào" ngay lúc này. Bản cũ lấy nó bằng
+   * `tc_->getPlan(...)` sau mỗi lần tính lệnh thành công (`move_base.cpp:2947`).
+   *
+   * @param[out] plan Rỗng nếu controller chưa sinh quỹ đạo nào.
+   */
+  virtual void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) = 0;
+
   /**
    * @brief Vận tốc đo được của robot, dùng làm dữ liệu vào cho lần tính lệnh kế tiếp.
    *

include/move_base2/runners/controller_runner.h

@@ -11,14 +11,17 @@
 
 #include <cstddef>
 #include <functional>
+#include <memory>
 #include <map>
 #include <string>
 #include <vector>
 
 #include <robot/node_handle.h>
-#include <robot_nav_core/base_local_planner.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_core2/local_planner.h>
 
 #include <move_base2/ports/controller_port.h>
+#include <move_base2/ports/pose_port.h>
 
 namespace robot_costmap_2d
 {
@@ -34,11 +37,31 @@ namespace move_base2
 
 /**
  * @class ControllerRunner
- * @brief Nạp và chạy local planner thế hệ 1 (`robot_nav_core::BaseLocalPlanner`) qua Boost.DLL.
+ * @brief Nạp và chạy local planner **thế hệ 2** (`robot_nav_core2::LocalPlanner`) qua Boost.DLL.
  *
- * Đây là chỗ duy nhất trong gói biết tới `robot_nav_core::BaseLocalPlanner`. Lõi quyết định chỉ thấy
+ * Đây là chỗ duy nhất trong gói biết tới `robot_nav_core2::LocalPlanner`. Lõi quyết định chỉ thấy
  * @ref ControllerPort.
  *
+ * ## Vì sao gen-2 chứ không phải gen-1
+ *
+ * Mọi local planner trong workspace này (`PriestLocalPlanner`, `HybridLocalPlanner`, …) implement
+ * `robot_nav_core2::LocalPlanner`; bản `move_base` cũ gọi tới chúng qua
+ * `robot_nav_core_adapter::LocalPlannerAdapter` — một `BaseLocalPlanner` bọc ngoài. Vì thế YAML có
+ * hai khoá riêng: `base_local_planner: LocalPlannerAdapter` (cái bọc) và
+ * `position_planner_name: PriestLocalPlanner` (planner thật).
+ *
+ * `move_base2` bỏ lớp bọc và gọi thẳng gen-2 — nhưng **phải nạp đúng kiểu gen-2**. Nạp một planner
+ * gen-2 qua `import_alias<BaseLocalPlanner::Ptr()>` sẽ "thành công": Boost.DLL là dlsym +
+ * reinterpret_cast, không kiểm kiểu nào qua ranh giới `.so`. Mọi lời gọi ảo sau đó trúng sai slot
+ * vtable — không lỗi biên dịch, không exception, chỉ segfault ở cycle đầu tiên. Đã xảy ra một lần.
+ *
+ * ## Ba việc mà lớp bọc gen-1 từng làm, nay làm ở đây
+ *
+ * Theo đúng `LocalPlannerAdapter`:
+ *  1. `setPlan` phải tách **goal** ra khỏi plan (pose cuối) và gọi `setGoalPose` trước `setPlan`;
+ *  2. `computeVelocityCommands` và `isGoalReached` của gen-2 cần **pose hiện tại** làm tham số;
+ *  3. giữ cờ "đang có goal" — gen-2 không tự biết, và tính lệnh khi chưa có goal là vô nghĩa.
+ *
  * ## Chạy đồng bộ, có chủ đích
  *
  * Khác @ref PlannerRunner, controller **không** cần thread riêng: nó phải trả lệnh trong đúng cycle
@@ -83,8 +106,8 @@ public:
    * @param initial_controller Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner.
    * @param[out] error         Lý do thất bại; chỉ ghi khi hàm trả false.
    */
-  bool configure(const robot::NodeHandle& nh, tf3::BufferCore* tf,
-                 robot_costmap_2d::Costmap2DROBOT* costmap,
+  bool configure(const robot::NodeHandle& nh, const std::shared_ptr<tf3::BufferCore>& tf,
+                 robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
                  const std::string& initial_controller, std::string& error);
 
   bool configured() const
@@ -101,6 +124,7 @@ public:
   bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override;
   bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override;
   bool isGoalReached() override;
+  void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) override;
   void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override;
   bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override;
   bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override;
@@ -116,24 +140,31 @@ private:
   /// @brief Một plugin đã nạp: factory phải sống cùng instance — vứt factory là để `.so` unload.
   struct Loaded
   {
-    std::function<robot_nav_core::BaseLocalPlanner::Ptr()> factory;
-    robot_nav_core::BaseLocalPlanner::Ptr instance;
+    std::function<robot_nav_core2::LocalPlanner::Ptr()> factory;
+    robot_nav_core2::LocalPlanner::Ptr instance;
   };
 
   /// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do).
-  robot_nav_core::BaseLocalPlanner* acquire(const std::string& name);
+  robot_nav_core2::LocalPlanner* acquire(const std::string& name);
 
-  /// @brief Áp lại trần vận tốc và sai số đã lưu lên controller vừa đổi sang.
-  void applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller);
+  /// @brief Áp lại trần vận tốc đã lưu lên controller vừa đổi sang.
+  void applyPendingLimits(robot_nav_core2::LocalPlanner* controller);
+
+  /// @brief Pose hiện tại dạng 2D. @return false khi mất TF — bên gọi phải dừng, không đi mù.
+  bool currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const;
 
   robot::NodeHandle nh_;
-  tf3::BufferCore* tf_ = nullptr;
+  std::shared_ptr<tf3::BufferCore> tf_;
   robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
+  const PosePort* pose_ = nullptr;  ///< Non-owning.
   bool configured_ = false;
 
   std::map<std::string, Loaded> controllers_;
   std::string active_name_;
-  robot_nav_core::BaseLocalPlanner* active_ = nullptr;  ///< Non-owning, trỏ vào @ref controllers_.
+  robot_nav_core2::LocalPlanner* active_ = nullptr;  ///< Non-owning, trỏ vào @ref controllers_.
+
+  /// Gen-2 không tự biết đang có goal hay không; tính lệnh khi chưa có goal là vô nghĩa.
+  bool has_active_goal_ = false;
 
   /**
    * Trần vận tốc và vận tốc đo được gần nhất.