DuongTD/move_base2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:41:32 +07:00
parent d8babff20b
commit 701d25f952
70 changed files with 5572 additions and 1146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/move_base2/bridges/mission_adapter_bridge.h
View File

@@ -33,7 +33,9 @@ namespace move_base2
* @class MissionAdapterBridge
* @brief Lớp nối duy nhất giữa `move_base2` và `mission_adapters`.
*
* Đây là file **duy nhất** trong gói include `mission_adapters`. Lõi quyết định chỉ thấy
* Cùng với @ref MissionLayer, đây là một trong **hai** file của gói include `mission_adapters`, và
* cả hai đều nằm trong `bridges/` — biên đó là chỗ duy nhất được phép biết tới framework mission.
* Lớp này lo phần **dịch contract**, `MissionLayer` lo phần **lắp ráp**. Lõi quyết định chỉ thấy
* @ref MissionPort và không biết framework mission nào đang chạy phía sau.
*
* Bắc qua hai interface cùng lúc:

179
include/move_base2/bridges/mission_layer.h Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — sở hữu và lắp ráp framework mission_adapters.
*
* Author: DuongTD
*********************************************************************/
#ifndef MOVE_BASE2_BRIDGES_MISSION_LAYER_H_
#define MOVE_BASE2_BRIDGES_MISSION_LAYER_H_
#include <cstddef>
#include <string>
#include <robot/node_handle.h>
#include <mission_adapters/event_processor.h>
#include <mission_adapters/mission_config.h>
#include <mission_adapters/mission_executor.h>
#include <mission_adapters/mission_manager.h>
#include <mission_adapters/plugin_registry.h>
#include <move_base2/bridges/mission_adapter_bridge.h>
namespace move_base2
{
/**
* @class MissionLayer
* @brief Chỗ dựng framework mission: registry plugin, hàng đợi, event thread, executor thread.
*
* @ref MissionAdapterBridge là lớp **dịch** giữa hai contract; lớp này là chỗ **lắp ráp** những thứ
* mà bridge cần có ở đầu kia. Tách ra vì hai việc hỏng theo hai kiểu khác nhau: bridge sai là sai
* ngữ nghĩa outcome, lắp ráp sai là thiếu thread hoặc sai thứ tự huỷ.
*
* ## Vì sao lớp này tồn tại
*
* Trước đây bridge nhận `MissionManager*` non-owning qua `attach()` mà **không ai cấp** — mission
* layer build ra `.so` nhưng chưa bao giờ được dựng lúc chạy, nên toàn bộ năng lực của nó (cắt
* order thành chặng, hàng đợi, base/horizon, orderUpdateId, mission timeout) nằm ngoài đường chạy
* thật. Lớp này đóng đúng khoảng trống đó.
*
* ## Vòng đời và thứ tự huỷ
*
* Thứ tự khai báo thành viên là thứ tự huỷ ngược: @ref executor_ và @ref events_ (hai thread) bị
* huỷ **trước** @ref manager_ và @ref registry_ mà chúng tham chiếu tới. Đảo thứ tự khai báo là
* thread còn sống gọi vào object đã huỷ — đúng nhóm lỗi shutdown F1–F7 đã tốn một buổi để truy.
*
* @note Không thread-safe cho phần cấu hình: @ref configure / @ref attach / @ref start / @ref stop
* chỉ được gọi từ thread khởi tạo. Các hàm đẩy sự kiện (@ref submitOrder, @ref cancel …)
* gọi được từ thread bất kỳ — chúng chỉ xếp sự kiện vào bus.
*/
class MissionLayer
{
public:
MissionLayer();
~MissionLayer();
MissionLayer(const MissionLayer&) = delete;
MissionLayer& operator=(const MissionLayer&) = delete;
/**
* @brief Nạp tham số vận hành và toàn bộ nguồn mission khai trong YAML.
* @param nh NodeHandle gốc.
* @param ns Namespace của mission layer (`MoveBase2Config::mission_namespace`).
* @param error Lý do cụ thể khi trả false.
* @return false nếu tham số không hợp lệ, hoặc **không nguồn nào** nạp được.
*
* Nạp hụt một vài nguồn không phải lỗi chặn: các nguồn còn lại vẫn dùng được và mỗi lỗi đã được
* @ref mission_adapters::PluginRegistry log kèm lý do. Chỉ khi không còn nguồn nào thì mission
* layer mới vô nghĩa — lúc đó bên gọi phải quay về đường navigation trực tiếp.
*/
bool configure(robot::NodeHandle& nh, const std::string& ns, std::string& error);
/**
* @brief Nối hai chiều với bridge: bridge báo outcome lên manager, executor đẩy chặng qua bridge.
*
* @param bridge Phải sống lâu hơn lớp này (trong `NavigationRuntime` cả hai là thành viên và
* bridge được khai báo trước).
*/
void attach(MissionAdapterBridge& bridge);
/// @brief Khởi động thread sự kiện và thread executor. No-op nếu @ref configure chưa thành công.
void start();
/// @brief Dừng và join hai thread. Gọi được nhiều lần.
void stop();
/// @brief Đã cấu hình xong và có ít nhất một nguồn mission.
bool active() const
{
return active_;
}
/// @brief Đang chạy: sự kiện đẩy vào sẽ được xử lý.
bool started() const
{
return started_;
}
/// @brief Có nguồn nào nhận @p schema này không — hỏi TRƯỚC khi định tuyến vào mission layer.
bool handles(const std::string& schema) const;
/**
* @brief Đẩy một VDA5050 Order vào mission layer.
* @return false nếu layer chưa chạy hoặc không có nguồn nào nhận schema `vda5050.order` — bên
* gọi phải tự xử lý order theo đường khác, KHÔNG được coi như đã nhận.
*
* Trả true chỉ có nghĩa "đã nhận vào hàng đợi sự kiện". Order hỏng bị adapter từ chối sau đó,
* trên thread sự kiện, kèm log nêu lý do — hàng đợi đang chạy không bị đụng tới (A1).
*/
bool submitOrder(const robot_protocol_msgs::Order& order);
/**
* @brief Đẩy một goal đơn lẻ vào mission layer.
* @return false nếu layer chưa chạy hoặc không có nguồn nào nhận schema `geometry.pose_stamped`.
*
* @note Đây là đường chuẩn của `NavigationServer::moveTo(PoseStamped)`: RViz, OPC-UA hay nguồn
* host nào gửi direct position goal đều phải đi qua `GoalSourceAdapter` để nhận cùng
* mission id/lifecycle với VDA5050. Các entry point mang profile riêng (`dockTo`,
* `moveStraightTo`, `rotateTo`) vẫn đi thẳng vì schema pose hiện không mang marker/profile.
*/
bool submitGoal(const robot_geometry_msgs::PoseStamped& goal);
/// @brief Huỷ chặng đang chạy và xoá sạch hàng đợi.
void cancel();
/// @brief Tạm dừng giao chặng mới. Chặng đang chạy do phía navigation tự tạm dừng.
void pause();
void resume();
/// @brief Dừng khẩn: xoá hàng đợi ngay, không xếp sau các sự kiện đang chờ.
void emergency();
void clearEmergency();
/// @brief Còn việc treo hay không — hàng đợi hoặc chặng đang chạy.
bool hasMission() const;
mission_adapters::MissionState state() const;
/// @brief Số nguồn mission đã đăng ký.
std::size_t sourceCount() const;
/**
* @brief Registry để test đăng ký nguồn giả mà không cần `.so` trên đĩa.
*
* Chỉ được gọi trước @ref start.
*/
mission_adapters::PluginRegistry& registry()
{
return registry_;
}
mission_adapters::MissionManager& manager()
{
return manager_;
}
/// @brief Đánh dấu layer dùng được sau khi test đã tự đăng ký nguồn qua @ref registry.
void markActiveForTesting();
private:
mission_adapters::MissionConfig config_;
mission_adapters::PluginRegistry registry_;
mission_adapters::MissionManager manager_;
/// Khai báo sau manager_/registry_: hai thread phải chết trước những gì chúng tham chiếu.
mission_adapters::EventProcessor events_;
mission_adapters::MissionExecutor executor_;
bool active_ = false;
bool started_ = false;
};
} // namespace move_base2
#endif // MOVE_BASE2_BRIDGES_MISSION_LAYER_H_

76
include/move_base2/config/move_base2_config.h
View File

@@ -25,7 +25,7 @@ namespace move_base2
*
* Ba tính chất bắt buộc, áp cho **mọi** tham số ở đây:
* 1. có default ngay tại khai báo (không có magic number rải trong code);
* 2. có đơn vị ghi tại chỗ khai báo;
* 2. có đơn vị ghi tại chỗ khai báo khi tham số mang đơn vị;
* 3. đi qua @ref validate — sai miền giá trị thì runtime **không khởi động**, thay vì chạy tiếp
* với một giá trị vô nghĩa.
*
@@ -46,6 +46,18 @@ struct MoveBase2Config
/// [s] Trần thời gian chờ một lần lập plan trước khi coi là hỏng. <= 0 = không giới hạn.
double planner_timeout = 5.0;
// --- Telemetry --------------------------------------------------------------------------------
/**
* [s] Chu kỳ in bảng thông số runtime (CPU theo thread, chi phí từng đoạn công việc, RSS) ra
* terminal. `0` = tắt hẳn: không đo, không in, không tốn gì.
*
* Mặc định tắt vì đây là công cụ chẩn đoán, không phải thứ chạy trên robot sản xuất — bảng in ở
* nhịp vài giây vẫn là log trong tiến trình điều khiển. Bật bằng `runtime_stats_period: 5.0`
* trong `move_base_common_params.yaml`.
*/
double runtime_stats_period = 0.0;
// --- Hành vi chuyển state --------------------------------------------------------------------
StateMachineConfig state_machine;
@@ -67,6 +79,22 @@ struct MoveBase2Config
ProfileBinding go_straight;
ProfileBinding rotate;
/// Alias global planner dự phòng, thử một lần sau khi planner profile active trả failure/plan rỗng.
std::string backup_global_planner_name;
/**
* Override global/local planner của docking theo marker, đọc từ root
* `docking_marker_profiles` trong maker_sources.yaml. Marker rỗng hoặc không có entry dùng
* cặp @ref docking mặc định; mỗi entry phải khai đủ cả hai planner.
*/
DockingMarkerProfiles docking_marker_profiles;
/// Lỗi schema của `docking_marker_profiles`, giữ lại để @ref validate chặn boot an toàn.
std::string docking_marker_profiles_error;
/// Giữ marker cho docking planner legacy; false cho profile docking dựa hoàn toàn vào goal_frame.
bool docking_requires_marker = true;
// --- Namespace cho các thành phần nạp plugin ---------------------------------------------------
/// Namespace chứa danh sách recovery behavior (`<ns>/behaviors`) trong YAML.
@@ -78,6 +106,22 @@ struct MoveBase2Config
/// Namespace chứa cấu hình mission layer.
std::string mission_namespace = "mission_adapters";
/**
* Dựng mission layer (`mission_adapters`) hay không.
*
* `true` (mặc định): VDA5050 Order đi qua mission layer — order được cắt thành từng chặng tại
* mỗi node có action, chỉ phần `released` được chạy, `orderUpdateId` nối tiếp thay vì chạy lại,
* và có `mission_timeout` làm lưới cuối.
*
* `false`: order đi thẳng xuống navigation như MỘT goal duy nhất (hành vi của move_base gen-1).
* Đây là đường lùi khi mission layer gây vấn đề trên hiện trường — đổi một khoá YAML, không phải
* build lại.
*
* @note Bật mà không nạp được nguồn nào thì runtime tự quay về đường trực tiếp **kèm log cảnh
* báo**: thiếu plugin không được phép biến thành robot đứng im không rõ lý do.
*/
bool mission_layer_enabled = true;
// --- Frame ------------------------------------------------------------------------------------
/// Frame mà goal được quy về trước khi lập plan.
@@ -86,6 +130,14 @@ struct MoveBase2Config
/// Frame gắn với thân robot.
std::string robot_base_frame = "base_link";
/**
* Chặn điều khiển bánh xe khi observation buffer của costmap điều khiển đã quá hạn.
*
* Default `true` = parity với move_base thế hệ 1 (`move_base.cpp:2720`). Xem
* @ref ControlLoopConfig::require_current_costmap về hệ quả khi tắt.
*/
bool require_current_costmap = true;
/**
* @brief Đọc toàn bộ tham số từ @p nh.
*
@@ -96,6 +148,20 @@ struct MoveBase2Config
*/
void fromNodeHandle(robot::NodeHandle& nh);
/**
* @brief Đọc schema runtime ở root với từng cặp planner độc lập:
*
* @code{.yaml}
* position:
* global_planner: CustomPlanner
* local_planner: HybridLocalPlanner
* @endcode
*
* Khác schema gen-1, `local_planner` là plugin `robot_nav_core2::LocalPlanner` thật; không đi
* qua `LocalPlannerAdapter`. Các tham số runtime chung vẫn nằm ở root cùng cấp với profile.
*/
void fromRootProfileNodeHandle(robot::NodeHandle& nh);
/**
* @brief Đọc theo schema move_base gen-1 (`move_base_common_params.yaml`, khoá ở root).
*
@@ -105,7 +171,7 @@ struct MoveBase2Config
* `base_global_planner` ở root;
* - `base_local_planner` (LocalPlannerAdapter) bị BỎ QUA có log: adapter là cầu nhúng planner
* gen-2 vào move_base gen-1, move_base2 gọi thẳng interface gen-2 qua ControllerPort;
* - `xy/yaw_goal_tolerance` ở root -> tolerance mặc định của cả bốn profile.
* - tolerance không thuộc move_base2: mỗi local planner tự đọc tolerance từ YAML riêng của nó.
*
* Hai khác biệt NGỮ NGHĨA được dịch tường minh (có log cảnh báo khi kích hoạt):
* 1. patience = 0: gen-1 nghĩa là "fail -> recovery NGAY" (mốc + 0 luôn ở quá khứ), gen-2 nghĩa
@@ -119,9 +185,9 @@ struct MoveBase2Config
void fromLegacyNodeHandle(robot::NodeHandle& nh);
/**
* @brief Tự nhận diện schema rồi đọc: có namespace `move_base2` -> schema mới (khoá gen-1 nếu
* còn nằm cạnh sẽ bị bỏ qua toàn bộ — KHÔNG trộn từng khoá giữa hai schema); không có
* nhưng thấy khoá gen-1 -> @ref fromLegacyNodeHandle; không thấy gì -> default + log.
* @brief Tự nhận diện schema rồi đọc, theo thứ tự: namespace `move_base2`, profile ở root
* (`position/local_planner`), rồi schema gen-1. Khi một schema đã được chọn, các khoá
* của schema khác bị bỏ qua toàn bộ — KHÔNG trộn từng khoá giữa chúng.
*
* Không trộn per-key là chủ đích: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu lỗi
* "sửa config mãi không ăn" đã ghi nhận với hai cây config trùng tên của workspace.

65
include/move_base2/control_loop.h
View File

@@ -11,6 +11,7 @@
#include <cstddef>
#include <cstdint>
#include <map>
#include <string>
#include <vector>
@@ -26,6 +27,7 @@
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/planner_port.h>
#include <move_base2/ports/pose_port.h>
#include <move_base2/ports/costmap_status_port.h>
#include <move_base2/ports/recovery_port.h>
namespace move_base2
@@ -47,12 +49,19 @@ struct ControlLoopDeps
ControllerPort* controller = nullptr;
RecoveryPort* recovery = nullptr;
MissionPort* mission = nullptr; ///< Có thể null.
/**
* Nguồn biết costmap còn hạn hay không. **Có thể null** — null nghĩa là không ai biết được, lõi
* coi dữ liệu là còn hạn và guard "không đi mù" không có hiệu lực. Mọi bộ test dùng cổng giả rơi
* vào nhánh này, nên hành vi của chúng không đổi.
*/
CostmapStatusPort* costmap_status = nullptr;
ActionPort* action = nullptr; ///< Có thể null (D8) — null thì yêu cầu có action bị từ chối.
};
/**
* @struct ProfileBinding
* @brief Ánh xạ một kiểu chuyển động sang cặp planner và sai số mặc định.
* @brief Ánh xạ một kiểu chuyển động sang cặp planner.
*
* Bảng này là thứ thay thế sáu entry point gần như giống hệt nhau của contract host cũ: chúng chỉ
* khác nhau ở đúng những trường dưới đây.
@@ -61,10 +70,11 @@ struct ProfileBinding
{
std::string global_planner_name; ///< Alias plugin global planner.
std::string local_planner_name; ///< Alias plugin local planner.
double default_xy_tolerance = 0.15; ///< [m]
double default_yaw_tolerance = 0.10; ///< [rad]
};
/// @brief Override cặp planner docking theo marker; marker không có entry thì dùng @ref docking.
using DockingMarkerProfiles = std::map<std::string, ProfileBinding>;
/**
* @struct ControlLoopConfig
* @brief Tham số của control loop.
@@ -81,12 +91,42 @@ struct ControlLoopConfig
/// tốc trong hệ thân xe, không phải hệ bản đồ hay odom.
std::string robot_base_frame = "base_link";
/**
* Chặn điều khiển bánh xe khi dữ liệu quan sát của costmap đã quá hạn.
*
* Default `true` = **đúng hành vi của move_base thế hệ 1** (`move_base.cpp:2720`): buffer hết hạn
* thì phát 0 và không cho lái, "we don't want to drive blind". Đặt `false` chỉ khi biết chắc
* `expected_update_rate` của các observation buffer đang cấu hình sai — tắt guard để robot chạy
* được là đổi một lỗi cấu hình lấy một robot đi mù.
*
* Không có tác dụng khi @ref ControlLoopDeps::costmap_status null.
*/
bool require_current_costmap = true;
/// Ánh xạ profile -> planner. Thiếu binding cho profile nào thì yêu cầu profile đó bị từ chối.
ProfileBinding position;
ProfileBinding docking;
ProfileBinding go_straight;
ProfileBinding rotate;
/**
* Global planner dự phòng dùng một lần cho mỗi request sau khi planner chính trả failure/plan rỗng.
* Chuỗi rỗng = tắt, giữ nguyên hành vi recovery hiện tại. Backup luôn nhận overload
* `makePlan(start, goal, plan)`: nhờ đó một planner tổng quát như SBPLLatticePlanner vẫn là
* đường lùi được cho position leg mang VDA5050 Order.
*/
std::string backup_global_planner_name;
/// Override cho profile docking. Không có entry hoặc marker rỗng -> dùng @ref docking.
DockingMarkerProfiles docking_marker_profiles;
/**
* Giữ contract `dockTo` cũ: `PNKXDockingLocalPlanner` cần marker để đọc `maker_name` lúc init.
* Đặt false khi profile docking nhận goal tuyệt đối/goal_frame (vd `HybridLocalPlanner`) và không
* đọc marker; vẫn validate marker khi caller cung cấp nó.
*/
bool docking_requires_marker = true;
bool validate(std::string& error) const;
std::string describe() const;
};
@@ -137,6 +177,18 @@ public:
*/
bool submit(const NavigationRequest& request, std::string& reason);
private:
/**
* @brief Quy đích đến muộn (`goal_frame` / `relative_distance`) về pose tuyệt đối.
* @return false kèm lý do nếu không quy được — chặng bị từ chối, không đoán.
*
* Chạy tại `submit`, nơi duy nhất vừa biết chặng vừa được kích hoạt vừa có `deps_.pose`. Mission
* layer sinh chặng lúc robot còn cách đó vài chục mét nên không thể quy sớm hơn.
*/
bool resolveDeferredGoal(NavigationRequest& request, std::string& reason) const;
public:
void requestPause();
void requestResume();
void requestCancel();
@@ -235,8 +287,8 @@ public:
void reset();
private:
/// @brief Binding cho một profile; nullptr nếu profile chưa được cấu hình.
const ProfileBinding* bindingFor(MotionProfile profile) const;
/// @brief Binding cho profile; docking tra override marker trước rồi mới dùng default.
const ProfileBinding* bindingFor(MotionProfile profile, const std::string& marker) const;
/**
* @brief Thu kết quả lập plan bất đồng bộ và quy nó thành @ref planner_feedback_.
@@ -277,6 +329,9 @@ private:
std::vector<robot_geometry_msgs::PoseStamped> latest_plan_;
bool planner_running_ = false;
/// True sau khi planner chính của request đã fail và backup được kích hoạt; không thử lại lần hai.
bool backup_global_planner_active_ = false;
/**
* Nhãn của yêu cầu đang chạy, cấp cho từng lượt lập plan.
*

45
include/move_base2/core/navigation_request.h
View File

@@ -10,6 +10,7 @@
#define MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_
#include <cstdint>
#include <limits>
#include <memory>
#include <string>
#include <vector>
@@ -40,30 +41,6 @@ enum class MotionProfile
/// @brief Tên profile dạng chuỗi, cho log và config.
const char* toString(MotionProfile profile);
/**
* @struct GoalTolerance
* @brief Sai số chấp nhận được tại đích.
*
* Quy ước: giá trị <= 0 nghĩa là "dùng default của profile trong config", không phải "yêu cầu sai số
* bằng 0". Quy ước này kế thừa từ contract host cũ (tham số mặc định 0.0) nên không đổi được.
*/
struct GoalTolerance
{
double xy = 0.0; ///< [m]
double yaw = 0.0; ///< [rad]
/// @brief Có ghi đè default của profile hay không.
bool hasXy() const
{
return xy > 0.0;
}
bool hasYaw() const
{
return yaw > 0.0;
}
};
/**
* @struct NavigationRequest
* @brief Một chặng navigation cần chạy.
@@ -85,8 +62,6 @@ struct NavigationRequest
/// Pose đích. Frame bất kỳ; phần nối dây chịu trách nhiệm đưa về global frame trước khi lập plan.
robot_geometry_msgs::PoseStamped goal;
GoalTolerance tolerance;
/**
* D8: action của mission, chạy SAU khi tới goal (hoặc ngay lập tức nếu @ref has_goal false),
* đúng thứ tự trong vector. Mission layer chép nguyên từ mission output, runtime không diễn giải
@@ -100,6 +75,24 @@ struct NavigationRequest
/// Order gốc nếu yêu cầu đến từ giao thức fleet; null nếu là goal trực tiếp.
std::shared_ptr<robot_protocol_msgs::Order> order;
/**
* Đích lấy từ TF frame này thay vì từ @ref goal. Rỗng = dùng @ref goal.
*
* Dành cho chặng mà đích **chưa biết lúc chặng được sinh ra**: bước dò phía trước tạo ra frame
* này, và `ControlLoop::submit` tra TF tại đúng thời điểm chặng được nhận rồi ghi kết quả vào
* @ref goal. Tra không được thì chặng bị **từ chối kèm lý do** — không đoán, không dùng goal cũ.
*/
std::string goal_frame;
/**
* Quãng đường tương đối [m] so với pose hiện tại, theo hướng thân robot. NaN = không dùng.
* Dương = tiến, âm = lùi.
*
* Cũng được quy ra @ref goal tuyệt đối tại `submit`, vì cùng một lý do: lúc mission layer sinh
* chặng thì robot còn chưa tới chỗ xuất phát của quãng đường đó.
*/
double relative_distance = std::numeric_limits<double>::quiet_NaN();
/**
* Số hiệu chặng do mission layer cấp. 0 = goal trực tiếp, không thuộc mission nào.
*

12
include/move_base2/core/state_machine.h
View File

@@ -9,6 +9,7 @@
#ifndef MOVE_BASE2_CORE_STATE_MACHINE_H_
#define MOVE_BASE2_CORE_STATE_MACHINE_H_
#include <array>
#include <cstddef>
#include <string>
@@ -99,6 +100,9 @@ struct StateMachineConfig
/// Cho phép chạy recovery hay không. false = mọi lỗi dẫn thẳng tới ABORTED.
bool recovery_enabled = true;
/// Route đã resolve của từng @ref RecoveryTrigger. Rỗng = mọi trigger dùng list legacy chung.
RecoveryRoutes recovery_routes;
/**
* @brief Kiểm miền giá trị.
* @param[out] error Mô tả tham số sai; chỉ được ghi khi hàm trả false.
@@ -260,7 +264,7 @@ public:
return config_;
}
/// @brief Chỉ số behavior sẽ chạy ở lần vào recovery kế tiếp. Dùng để assert trong test.
/// @brief Index behavior đang chạy / sẽ chạy ở lần recovery kế tiếp. Dùng để assert trong test.
std::size_t nextRecoveryIndex() const
{
return recovery_index_;
@@ -320,6 +324,10 @@ private:
void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now,
const char* reason, StateMachineOutput& out);
/// Cursor của route cho @p trigger trong request hiện hành.
std::size_t& recoveryRouteCursor(RecoveryTrigger trigger);
const std::size_t& recoveryRouteCursor(RecoveryTrigger trigger) const;
StateMachineConfig config_;
bool initialized_ = false;
@@ -333,6 +341,8 @@ private:
robot::Time last_oscillation_reset_;
std::size_t recovery_index_ = 0;
RecoveryTrigger active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
std::array<std::size_t, 3> recovery_route_cursors_{{ 0, 0, 0 }};
int planning_retries_ = 0;
/// D8 — hình dạng của yêu cầu hiện tại, chốt tại cycle nhận yêu cầu từ StateMachineInput.

9
include/move_base2/io/costmap_exporter.h
View File

@@ -13,6 +13,7 @@
#include <string>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <move_base2/io/runtime_stats.h>
#include <robot_nav_msgs/OccupancyGrid.h>
namespace robot_costmap_2d
@@ -74,7 +75,15 @@ public:
void fill(robot_nav_msgs::OccupancyGrid& grid, robot_map_msgs::OccupancyGridUpdate& update,
bool& is_updated);
/// @brief Gắn telemetry đo chi phí kết xuất lưới cho rviz (non-owning, null = tắt).
/// Hàm này chạy trên ros::Timer của host, không phải control thread.
void attachTelemetry(RuntimeStats* telemetry);
private:
/// Telemetry non-owning, null = tắt đo.
RuntimeStats* telemetry_ = nullptr;
RuntimeStats::SectionId section_fill_ = RuntimeStats::kInvalidSection;
void prepareGridLocked();
mutable std::mutex mutex_;

200
include/move_base2/io/runtime_stats.h Normal file
View File

@@ -0,0 +1,200 @@
/**
* @file runtime_stats.h
* @brief Thu thập và in định kỳ ra terminal chi phí CPU/bộ nhớ của từng thành phần trong tiến trình.
*
* Bài toán mà file này giải: cả navigation stack chạy trong **một tiến trình** cùng với host ROS,
* nên `top`/`htop` chỉ cho biết tiến trình ăn bao nhiêu, không cho biết *thành phần nào* ăn. Muốn
* biết được, phải đo từ bên trong:
*
* - **Theo thread** — mỗi thread có bộ đếm CPU riêng ở `/proc/self/task/<tid>/stat`. Thành phần
* nào sở hữu thread riêng (control loop, thread lập plan, hai vòng cập nhật costmap) thì đọc
* thẳng được chi phí của nó. Thread không đăng ký được gộp vào một dòng "không đăng ký" — con số
* đó chính là phần thuộc về host, và nó phải hiện ra chứ không được biến mất.
* - **Theo đoạn công việc** (@ref RuntimeStats::SectionId) — thứ chạy *bên trong* một thread có sẵn
* thì không tách được bằng bộ đếm của kernel. Ví dụ chi phí của local planner nằm lẫn trong
* control thread; chỉ bấm giờ quanh đúng lời gọi plugin mới tách được.
*
* @note Bộ đếm CPU đọc từ `/proc` nên phần theo thread chỉ có trên Linux. Nơi khác vẫn biên dịch và
* chạy được, chỉ là cột CPU% trống — phần đo theo đoạn công việc dùng `std::chrono` nên luôn
* có.
* @note Đồng hồ dùng ở đây là `steady_clock` (giờ tường), **không** phải `robot::Time`: đây là công
* cụ đo hiệu năng, nó phải đúng cả khi sim chạy nhanh/chậm hơn thời gian thật hoặc bị tạm dừng.
* @note Không cấp phát bộ nhớ trên đường nóng: đoạn công việc được đăng ký **một lần** lúc cấu hình
* và trả về một chỉ số; mỗi lần ghi chỉ cộng dồn vào phần tử vector đã có.
*/
#ifndef MOVE_BASE2_IO_RUNTIME_STATS_H_
#define MOVE_BASE2_IO_RUNTIME_STATS_H_
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <mutex>
#include <string>
#include <vector>
namespace move_base2
{
/**
* @class RuntimeStats
* @brief Bộ đếm dùng chung cho mọi thành phần của runtime, in bảng theo chu kỳ.
*
* Vòng đời và quyền sở hữu: đối tượng này do @ref NavigationServer sở hữu và sống lâu hơn mọi
* runner. Các runner giữ con trỏ **non-owning, cho phép null** — null nghĩa là telemetry tắt, và
* mọi lời gọi trở thành no-op. Đó cũng là đường mà test đi: không cấu hình telemetry thì không có
* gì được đo và không có gì được in.
*
* Thread-safety: @ref record và @ref registerCurrentThread gọi được từ thread bất kỳ (có mutex).
* @ref tick và @ref render chỉ nên gọi từ control thread. @ref beginThreadCapture /
* @ref endThreadCapture phải chạy trên cùng một thread và không được lồng nhau.
*/
class RuntimeStats
{
public:
using SectionId = std::size_t;
/// Chỉ số trả về khi telemetry tắt; @ref record và @ref ScopedSection bỏ qua nó.
static constexpr SectionId kInvalidSection = static_cast<SectionId>(-1);
/**
* @param period_seconds [s] Chu kỳ in bảng. `<= 0` = tắt hẳn telemetry (không đo, không in).
*/
explicit RuntimeStats(double period_seconds);
/// @brief Telemetry có bật không. Tắt thì mọi hàm còn lại là no-op rẻ tiền.
bool enabled() const
{
return period_seconds_ > 0.0;
}
/**
* @brief Đăng ký một đoạn công việc và nhận chỉ số của nó. Gọi MỘT LẦN lúc cấu hình.
* @param name Tên hiển thị, nên theo dạng `thành_phần.việc` (`controller.compute`).
* @return Chỉ số dùng cho @ref record; @ref kInvalidSection nếu telemetry tắt.
*/
SectionId section(const std::string& name);
/// @brief Cộng dồn một lần thực thi của đoạn @p id. An toàn khi @p id không hợp lệ.
void record(SectionId id, std::int64_t nanoseconds);
/**
* @brief Gắn nhãn cho thread ĐANG chạy. Phải gọi từ chính thread cần đo.
*
* Gọi hai lần cho cùng một thread thì lần sau ghi đè nhãn — thread bị tái sử dụng vẫn hiển thị
* đúng chủ sở hữu hiện tại.
*/
void registerCurrentThread(const std::string& label);
/**
* @brief Mở một cửa sổ chụp thread, dùng cho thành phần TỰ tạo thread của nó.
*
* Costmap tạo thread cập nhật ngay trong constructor và không phơi ra tid. Cách duy nhất để gọi
* đúng tên nó mà không phải sửa gói costmap: chụp danh sách tid trước khi dựng, chụp lại sau khi
* dựng xong, và mọi tid mới xuất hiện thuộc về thành phần vừa dựng.
*
* @warning Cửa sổ chụp phải bao trọn phần dựng và **không được có thành phần khác dựng thread
* song song** trong lúc đó, nếu không nhãn sẽ gán nhầm. Trong runtime này mọi lời gọi
* đều nằm trên đường khởi tạo tuần tự, nên điều kiện đó thoả.
*/
void beginThreadCapture();
/// @brief Đóng cửa sổ chụp và gán @p label cho mọi thread mới xuất hiện. Xem @ref beginThreadCapture.
void endThreadCapture(const std::string& label);
/**
* @brief Gọi mỗi cycle từ control thread; in bảng khi hết chu kỳ.
* @return true nếu vừa in ở lần gọi này.
*/
bool tick();
/**
* @brief Dựng bảng thống kê của cửa sổ hiện tại và mở cửa sổ mới.
*
* Tách khỏi @ref tick để test kiểm được nội dung mà không phải chờ hết chu kỳ thật.
*/
std::string render();
private:
struct Section
{
std::string name;
std::uint64_t calls = 0;
std::int64_t total_ns = 0;
std::int64_t max_ns = 0;
};
struct Thread
{
long tid = 0;
std::string label;
std::uint64_t last_cpu_ticks = 0;
};
/// Đọc utime+stime của một thread [tick của kernel]; 0 nếu không đọc được.
static std::uint64_t readThreadCpuTicks(long tid);
/// Đọc utime+stime của cả tiến trình [tick của kernel].
static std::uint64_t readProcessCpuTicks();
/// RSS hiện tại [byte]; 0 nếu không đọc được.
static std::uint64_t readProcessRssBytes();
/// Danh sách tid đang tồn tại của tiến trình.
static std::vector<long> listThreadIds();
const double period_seconds_;
const double ticks_per_second_;
mutable std::mutex mutex_;
std::vector<Section> sections_;
std::vector<Thread> threads_;
std::vector<long> capture_before_;
bool capturing_ = false;
std::chrono::steady_clock::time_point window_start_;
std::uint64_t last_process_cpu_ticks_ = 0;
std::uint64_t last_rss_bytes_ = 0;
};
/**
* @class ScopedSection
* @brief Bấm giờ một đoạn công việc theo phạm vi khối lệnh.
*
* An toàn khi @p stats là null hoặc @p id không hợp lệ — đó là trạng thái bình thường khi telemetry
* tắt, không phải lỗi.
*/
class ScopedSection
{
public:
ScopedSection(RuntimeStats* stats, RuntimeStats::SectionId id)
: stats_(id == RuntimeStats::kInvalidSection ? nullptr : stats)
, id_(id)
{
// Chỉ đọc đồng hồ khi thật sự đo. Telemetry tắt là trạng thái mặc định trên robot thật, và
// các ScopedSection này nằm trong vòng điều khiển — không được trả giá cho thứ đang tắt.
if (stats_ != nullptr)
{
start_ = std::chrono::steady_clock::now();
}
}
~ScopedSection()
{
if (stats_ == nullptr)
{
return;
}
const auto elapsed = std::chrono::steady_clock::now() - start_;
stats_->record(id_, std::chrono::duration_cast<std::chrono::nanoseconds>(elapsed).count());
}
ScopedSection(const ScopedSection&) = delete;
ScopedSection& operator=(const ScopedSection&) = delete;
private:
RuntimeStats* stats_;
RuntimeStats::SectionId id_;
std::chrono::steady_clock::time_point start_;
};
} // namespace move_base2
#endif // MOVE_BASE2_IO_RUNTIME_STATS_H_

17
include/move_base2/io/sensor_gateway.h
View File

@@ -13,6 +13,7 @@
#include <memory>
#include <string>
#include <move_base2/io/runtime_stats.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/LaserScan.h>
@@ -186,6 +187,15 @@ public:
stats_ = SensorGatewayStats();
}
/**
* @brief Gắn bộ telemetry để đo chi phí nạp cảm biến (non-owning, null = tắt).
*
* Đường nạp này chạy trên **thread callback của host**, không phải control thread — nên chi phí
* của nó không xuất hiện ở dòng `move_base2/control` mà nằm trong phần "(không đăng ký)". Đo bằng
* đoạn công việc là cách duy nhất tách được nó ra khỏi phần còn lại của host.
*/
void attachTelemetry(RuntimeStats* telemetry);
private:
/// @brief Cảnh báo lúc gắn nếu costmap có layer kiểu ObstacleLayer thuần — chúng sẽ không nhận gì.
void warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap* costmap, const char* which) const;
@@ -199,6 +209,13 @@ private:
std::unique_ptr<laser_filter::LaserScanSOR> laser_sor_;
SensorGatewayStats stats_;
/// Telemetry non-owning, null = tắt đo. Khác hẳn @ref stats_ (bộ đếm mẫu bị bỏ của chính gateway).
RuntimeStats* telemetry_ = nullptr;
RuntimeStats::SectionId section_static_map_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_laser_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_cloud_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_depth_ = RuntimeStats::kInvalidSection;
};
} // namespace move_base2

95
include/move_base2/navigation_runtime.h
View File

@@ -11,11 +11,14 @@
#include <memory>
#include <string>
#include <vector>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <move_base2/bridges/mission_adapter_bridge.h>
#include <move_base2/bridges/mission_layer.h>
#include <move_base2/config/move_base2_config.h>
#include <move_base2/io/runtime_stats.h>
#include <move_base2/control_loop.h>
#include <move_base2/io/costmap_exporter.h>
#include <move_base2/ports/clock_port.h>
@@ -55,6 +58,12 @@ public:
costmap_ = costmap;
}
/// @brief Buffer TF để tra frame lạ. Non-owning; null = @ref lookupPose luôn trả false.
void setTf(const std::shared_ptr<tf3::BufferCore>& tf)
{
tf_ = tf;
}
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
{
if (costmap_ == nullptr)
@@ -64,8 +73,45 @@ public:
return costmap_->getRobotPose(pose);
}
bool lookupPose(const std::string& frame,
robot_geometry_msgs::PoseStamped& pose) const override;
private:
robot_costmap_2d::Costmap2DROBOT* costmap_;
std::shared_ptr<tf3::BufferCore> tf_;
};
/**
* @class CostmapStatusAdapter
* @brief Cổng "costmap còn hạn không" lấy từ costmap thật.
*
* Trỏ vào costmap **điều khiển** (local), giống hệt bản cũ (`move_base.cpp:2720` hỏi
* `controller_costmap_robot_`): đó là costmap mà local planner tránh vật cản trên đó, nên nó mới là
* cái quyết định robot có được lái hay không. Costmap global cũ đi thì chỉ ảnh hưởng chất lượng
* plan, và state machine đã có `planner_patience` lo phần đó.
*
* @note Costmap null trả **false** — không biết thì không cho chạy.
*/
class CostmapStatusAdapter final : public CostmapStatusPort
{
public:
explicit CostmapStatusAdapter(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr)
: costmap_(costmap)
{
}
void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap)
{
costmap_ = costmap;
}
bool isCurrent() const override
{
return costmap_ != nullptr && costmap_->isCurrent();
}
private:
robot_costmap_2d::Costmap2DROBOT* costmap_; ///< non-owning
};
/**
@@ -145,6 +191,17 @@ public:
/// @brief Dừng cập nhật costmap.
void stop();
/**
* @brief Cập nhật footprint cho cả hai costmap và làm mới cache collision của local planner.
*
* Có thể gọi trong pha khởi tạo, sau @ref buildCostmaps nhưng trước @ref buildRunners: khi đó chỉ
* cập nhật hai costmap, để local planner đầu tiên đọc đúng footprint trong `initialize()`. Khi
* runtime đã dựng xong, hàm phải được gọi từ control thread và sẽ refresh cache local planner.
* Hai costmap có footprint riêng; chỉ đổi một bên sẽ làm planner global và local controller dùng
* hai hình robot khác nhau.
*/
bool setRobotFootprint(const std::vector<robot_geometry_msgs::Point>& footprint);
/// @brief Bộ cổng để bơm vào @ref ControlLoop. Rỗng nếu chưa @ref build.
ControlLoopDeps deps();
@@ -153,6 +210,17 @@ public:
return config_;
}
/**
* @brief Bộ telemetry dùng chung. Null cho tới khi @ref buildCostmaps chạy xong.
*
* Non-owning theo hướng người dùng: runtime sở hữu, bên gọi chỉ mượn. Trả về null vẫn hợp lệ —
* mọi hàm của @ref RuntimeStats an toàn với con trỏ null ở phía người gọi (@ref ScopedSection).
*/
RuntimeStats* stats()
{
return stats_.get();
}
robot_costmap_2d::Costmap2DROBOT* globalCostmap()
{
return global_costmap_.get();
@@ -168,6 +236,18 @@ public:
return mission_;
}
/**
* @brief Framework mission đứng sau bridge.
*
* `missionLayer().started()` là câu hỏi "order có được cắt thành chặng không". False nghĩa là
* layer bị tắt bằng config hoặc không nạp được nguồn nào — bên gọi phải tự đưa order xuống
* navigation theo đường trực tiếp.
*/
MissionLayer& missionLayer()
{
return mission_layer_;
}
PlannerRunner& planner()
{
return planner_;
@@ -197,9 +277,21 @@ private:
// 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.
/// Telemetry của cả runtime. Luôn tồn tại; tắt hay bật do `runtime_stats_period` quyết định.
/// Dựng trong buildCostmaps() ngay sau khi đọc config, vì nó phải chụp được thread mà costmap tạo.
std::unique_ptr<RuntimeStats> stats_;
/// Guard "không đi mù": hỏi costmap điều khiển xem observation buffer còn hạn không.
CostmapStatusAdapter costmap_status_;
std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> global_costmap_;
std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> local_costmap_;
/// Footprint đang thực sự có hiệu lực ở từng costmap; dùng để rollback khi local planner không
/// dựng lại được cache collision của footprint mới.
std::vector<robot_geometry_msgs::Point> global_footprint_;
std::vector<robot_geometry_msgs::Point> local_footprint_;
SystemClock clock_;
/**
@@ -222,6 +314,9 @@ private:
RecoveryRunner recovery_;
ActionRunner action_;
MissionAdapterBridge mission_;
/// Khai báo SAU bridge: hai thread của layer gọi vào bridge, nên chúng phải chết trước nó.
MissionLayer mission_layer_;
};
} // namespace move_base2

38
include/move_base2/navigation_server.h
View File

@@ -21,6 +21,7 @@
#include <move_base2/control_loop.h>
#include <move_base2/core/navigation_request.h>
#include <move_base2/io/runtime_stats.h>
#include <move_base2/io/sensor_gateway.h>
#include <move_base2/navigation_runtime.h>
@@ -243,6 +244,14 @@ private:
*/
void pushHostInputsToController();
/**
* @brief Áp footprint host vừa đặt vào runtime trên control thread.
*
* `setRobotFootprint()` là entry point host, có thể chạy song song với plugin và map-update
* thread. Vì vậy nó chỉ ghi pending state; hàm này mới gọi costmap/controller.
*/
void applyPendingFootprint();
/**
* @brief Chuyển các yêu cầu pause/resume/cancel mà host đã đặt xuống lõi.
*
@@ -251,6 +260,17 @@ private:
*/
void drainLifecycleRequests();
/**
* @brief Huỷ CHỈ chặng đang chạy, không đụng hàng đợi mission.
*
* Đây là đường mà mission layer dùng để dừng navigation (`MissionAdapterBridge::cancelActive`).
* Nó không được gọi `MissionLayer::cancel()`: yêu cầu vừa đi ra từ chính mission layer.
*/
void requestLoopCancel();
/// @brief Mission layer còn chặng đang chạy hoặc còn hàng đợi hay không.
bool missionHasPendingWork() const;
/**
* @brief Điền plan và footprint vào dữ liệu xuất cho host.
*
@@ -285,6 +305,13 @@ private:
std::unique_ptr<NavigationRuntime> runtime_;
/// Control thread — thread DUY NHẤT chạy control loop và phát cmd_vel.
/// Telemetry mượn từ @ref NavigationRuntime (non-owning, null = tắt). Runtime sống lâu hơn control
/// thread vì @ref shutdown dừng thread trước khi thả runtime.
RuntimeStats* stats_ = nullptr;
RuntimeStats::SectionId section_cycle_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_step_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_cache_plans_ = RuntimeStats::kInvalidSection;
std::thread control_thread_;
std::atomic<bool> control_thread_running_{ false };
robot::TFListenerPtr tf_;
@@ -293,6 +320,7 @@ private:
mutable std::mutex data_mutex_;
std::vector<robot_geometry_msgs::Point> footprint_;
bool footprint_pending_ = false;
std::map<std::string, robot_sensor_msgs::DepthCameraData::ConstPtr> depth_camera_data_;
/// Frame đóng dấu lên lệnh vận tốc gửi host. Chép từ config lúc @ref configureLoop.
@@ -331,6 +359,16 @@ private:
bool resume_requested_ = false;
bool cancel_requested_ = false;
/**
* Huỷ có lan tới cả hàng đợi mission hay không.
*
* Tách khỏi @ref cancel_requested_ vì hai nguồn huỷ có ý nghĩa khác nhau: huỷ từ HOST là "bỏ cả
* order", còn huỷ do chính mission layer yêu cầu (`MissionAdapterBridge::cancelActive`) chỉ là
* "dừng chặng đang chạy". Gộp làm một thì lời gọi thứ hai vòng ngược lên mission layer đúng cái
* vừa phát ra nó.
*/
bool mission_cancel_requested_ = false;
std::string last_reject_reason_;
};

20
include/move_base2/ports/controller_port.h
View File

@@ -40,11 +40,23 @@ public:
virtual bool swapPlanner(const std::string& planner_name) = 0;
/**
* @brief Đặt sai số chấp nhận tại đích cho yêu cầu hiện tại.
* @param xy_m [m]
* @param yaw_rad [rad]
* @brief Chọn marker cho chặng docking. Gọi TRƯỚC @ref swapPlanner của chính chặng đó.
*
* Kênh marker của bản cũ là param server: `dockTo` validate marker với `maker_sources` rồi
* `setParam("maker_name", marker)` (`move_base.cpp:1161-1173`), và docking local planner đọc lại
* MỘT lần trong `initialize()` (`pnkx_docking_local_planner.cpp:getMaker`). Bản cũ dlopen lại
* planner mỗi lần dock nên luôn đọc được giá trị mới; runner nào cache instance thì phải tự lo
* việc init lại khi marker đổi — đó là lý do hàm này thuộc port chứ không phải một setParam rời.
*
* @return false nếu marker không hợp lệ (không có trong `maker_sources`) — bên gọi phải từ chối
* yêu cầu, như bản cũ trả REJECTED.
*
* Default trả true (không làm gì): chỉ hiện thực thật (ControllerRunner) mới có param tree.
*/
virtual void setTolerance(double xy_m, double yaw_rad) = 0;
virtual bool setDockingMarker(const std::string& /*marker*/)
{
return true;
}
/**
* @brief Nạp plan mới.

48
include/move_base2/ports/costmap_status_port.h Normal file
View File

@@ -0,0 +1,48 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cổng hỏi costmap còn "tươi" hay không.
*
* Author: DuongTD
*********************************************************************/
#ifndef MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_
#define MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_
namespace move_base2
{
/**
* @class CostmapStatusPort
* @brief Cho lõi biết dữ liệu quan sát của costmap còn hạn hay đã cũ.
*
* Vì sao đây là một cổng riêng chứ không đọc thẳng costmap: lõi không được include
* `robot_costmap_2d` (cùng lý do với `RecoveryPort` và `recovery_core`). Cổng này là **tuỳ chọn** —
* `ControlLoopDeps::costmap_status` null nghĩa là không có nguồn nào biết được, và lõi coi dữ liệu
* là còn hạn. Mọi test cổng-giả có sẵn vì thế không đổi hành vi.
*
* Bối cảnh: `move_base` thế hệ 1 có đúng guard này ngay trong `executeCycle`
* (`move_base.cpp:2720`): observation buffer hết hạn thì phát vận tốc 0 và không cho điều khiển
* bánh xe — "we don't want to drive blind". move_base2 dựng lại theo mô hình cổng thay vì gọi thẳng
* costmap, nhưng ngữ nghĩa giữ nguyên.
*
* @note "Hết hạn" ở đây là quyết định của costmap (mỗi `ObservationBuffer` có
* `expected_update_rate` riêng), không phải của lõi. Lõi chỉ hỏi và tuân theo.
*/
class CostmapStatusPort
{
public:
virtual ~CostmapStatusPort() = default;
/**
* @brief Dữ liệu quan sát của costmap dùng cho điều khiển có còn hạn không.
*
* @return false nghĩa là **không được lái robot ở cycle này**. Hiện thực phải trả false khi không
* chắc — mù mà vẫn chạy là dạng hỏng nguy hiểm hơn hẳn dừng nhầm.
*/
virtual bool isCurrent() const = 0;
};
} // namespace move_base2
#endif // MOVE_BASE2_PORTS_COSTMAP_STATUS_PORT_H_

19
include/move_base2/ports/pose_port.h
View File

@@ -9,6 +9,8 @@
#ifndef MOVE_BASE2_PORTS_POSE_PORT_H_
#define MOVE_BASE2_PORTS_POSE_PORT_H_
#include <string>
#include <robot_geometry_msgs/PoseStamped.h>
namespace move_base2
@@ -31,6 +33,23 @@ public:
virtual ~PosePort() = default;
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0;
/**
* @brief Pose của một TF frame BẤT KỲ, quy về global frame.
* @return false nếu không tra được (frame chưa tồn tại, TF quá hạn, hoặc cổng không hỗ trợ).
*
* Dùng cho chặng có `NavigationRequest::goal_frame`: đích của nó không đến từ order mà từ một
* frame do bước dò sinh ra, và chỉ biết được tại thời điểm chặng được nhận.
*
* Default trả **false** thay vì thuần ảo, để mọi cổng giả sẵn có không phải sửa: cổng nào không
* hỗ trợ thì `ControlLoop::submit` từ chối chặng kèm lý do — an toàn hơn là im lặng dùng goal rỗng.
* @p pose không được ghi khi hàm trả false.
*/
virtual bool lookupPose(const std::string& /*frame*/,
robot_geometry_msgs::PoseStamped& /*pose*/) const
{
return false;
}
};
} // namespace move_base2

22
include/move_base2/ports/recovery_port.h
View File

@@ -34,6 +34,28 @@ enum class RecoveryTrigger
const char* toString(RecoveryTrigger trigger);
/**
* @struct RecoveryRoutes
* @brief Các route recovery đã resolve từ tên instance YAML sang index trong @ref RecoveryPort.
*
* Registry/plugin chỉ biết danh sách behavior. Policy "lỗi nào thử behavior nào trước" thuộc
* move_base2, nên @ref RecoveryRunner parse `recovery/routes` sau khi registry đã nạp xong rồi
* chuyển tên instance thành index ở đây. State machine chỉ nhìn thấy index — giữ lõi thuần, không
* phụ thuộc YAML hay recovery_core.
*
* Ba vector để rỗng cùng lúc nghĩa là legacy fallback: mọi trigger dùng toàn bộ behavior theo thứ
* tự registry. Khi một route được khai thì cả ba route phải có ít nhất một index hợp lệ.
*/
struct RecoveryRoutes
{
std::vector<std::size_t> planning_failed;
std::vector<std::size_t> controlling_failed;
std::vector<std::size_t> oscillation;
const std::vector<std::size_t>& forTrigger(RecoveryTrigger trigger) const;
bool empty() const;
};
/**
* @enum RecoveryOutputKind
* @brief Behavior đó có lái robot hay không.

79
include/move_base2/runners/action_handler.h
View File

@@ -1,79 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — contract cho plugin thực thi một loại action.
*
* Author: DuongTD
*********************************************************************/
#ifndef MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
#define MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
#include <memory>
#include <string>
#include <vector>
// robot_protocol_msgs/Action.h khai boost::shared_ptr nhưng không tự include — phải nạp trước nó,
// nếu không translation unit nào include Action.h đầu tiên sẽ hỏng.
#include <boost/shared_ptr.hpp>
#include <robot/node_handle.h>
#include <robot/time.h>
#include <robot_protocol_msgs/Action.h>
#include <move_base2/ports/action_port.h>
namespace move_base2
{
/**
* @class ActionHandler
* @brief Thực thi một hoặc nhiều loại action (`actionType` của VDA5050).
*
* Tick-based, cùng mô hình với recovery behavior và cùng lý do (D5): handler được gọi từ **control
* thread** ở `controller_frequency`, nên nó **không được block**. Chờ thiết bị thì giữ state nội bộ
* và trả `kRunning`; nhờ vậy cancel/pause/emergency luôn được phản hồi trong một cycle.
*
* @warning **Handler phải tự timeout.** Đây là tầng 1 của contract 3 tầng và là tầng chính: chỉ
* handler biết ngưỡng đúng cho thiết bị của nó ("nâng kệ quá 20 s là bất thường" khác hẳn
* "sạc 30 phút là bình thường"). `action_patience` của state machine chỉ là lưới cuối và
* mặc định tắt. Một handler trả `kRunning` vĩnh viễn là handler viết sai contract.
*
* @note Trong lúc action chạy, **không ai được phát cmd_vel** (D8). Action cần chuyển động phải
* được mô hình hoá thành motion profile của navigation, không phải làm trong handler.
*/
class ActionHandler
{
public:
using Ptr = std::shared_ptr<ActionHandler>;
virtual ~ActionHandler() = default;
/**
* @brief Cấu hình một lần.
* @param name Tên instance, dùng cho log.
* @param nh NodeHandle **đã được caller scope sẵn** vào namespace param của instance này.
* @return false nếu không chạy được với cấu hình này.
*/
virtual bool configure(const std::string& name, robot::NodeHandle& nh) = 0;
/// @brief Các `actionType` mà handler này nhận. Rỗng = không nhận gì (registry sẽ từ chối nạp).
virtual std::vector<std::string> supportedActionTypes() const = 0;
/**
* @brief Bắt đầu thực thi @p action.
* @param now Thời điểm hiện tại — mốc cho timeout của chính handler.
* @return false = từ chối khởi động; bên gọi coi như action thất bại và **không** tick tiếp.
*/
virtual bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) = 0;
/// @brief Một control cycle. Chỉ được gọi sau khi @ref start trả true.
virtual ActionTick update(const robot::Time& now) = 0;
/// @brief Yêu cầu dừng an toàn. Handler phải đưa thiết bị về trạng thái an toàn, không treo.
virtual void cancel() = 0;
};
} // namespace move_base2
#endif // MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_

67
include/move_base2/runners/action_runner.h
View File

@@ -2,7 +2,7 @@
*
* Software License Agreement (BSD License)
*
* move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
* move_base2 — hiện thực ActionPort bằng framework action_core.
*
* Author: DuongTD
*********************************************************************/
@@ -10,23 +10,31 @@
#define MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_
#include <cstddef>
#include <functional>
#include <map>
#include <string>
#include <vector>
#include <action_core/action_registry.h>
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/runners/action_handler.h>
namespace move_base2
{
/**
* @class ActionRunner
* @brief Bảng tra `actionType` -> handler, nạp từ YAML bằng Boost.DLL.
* @brief Nối @ref ActionPort với framework `action_core`.
*
* Cấu hình mong đợi:
* Đây là file **duy nhất** trong gói include `action_core`, cùng vai trò mà `RecoveryRunner` giữ
* với `recovery_core`: lõi quyết định chỉ thấy @ref ActionPort và không biết framework nào đang
* chạy phía sau. Việc nạp plugin, tra theo `actionType` và giữ `.so` sống thuộc về
* `action_core::ActionRegistry`; lớp này chỉ làm ba việc mà registry cố ý không làm:
*
* 1. **giữ đồng hồ** — registry không biết thời gian, handler thì cần mốc để tự timeout;
* 2. **nhớ action đang chạy** — registry là bảng tra, không có khái niệm "đang chạy";
* 3. **dịch kiểu** — `action_core::ActionTick` sang @ref ActionTick của port.
*
* Cấu hình mong đợi (chi tiết ở `action_core::ActionRegistry`):
*
* @code{.yaml}
* actions:
@@ -34,14 +42,13 @@ namespace move_base2
* - {name: noop, type: NoopActionHandler}
* noop:
* action_types: [wait, pick, drop]
* duration: 0.0 # [s]
*
* NoopActionHandler:
* library_path: libmove_base2_noop_action_handler
* library_path: libaction_core_noop_action_handler
* @endcode
*
* Danh sách handler **được phép rỗng**: một hệ không có thiết bị nào thì mọi mission đều là
* nav-only, và khi đó `ControlLoop::submit` đã từ chối yêu cầu mang action ngay tại cửa.
* nav-only, và @ref start sẽ từ chối kèm lý do nêu đích danh `actionType` không ai nhận.
*
* @note Không thread-safe. Chỉ control thread được gọi.
*/
@@ -60,11 +67,20 @@ public:
/// @brief Namespace YAML chứa `<ns>/handlers`. Mặc định "actions".
void setNamespace(const std::string& ns);
/**
* @brief Cổng môi trường cấp cho handler (TF, frame). Đặt TRƯỚC @ref configure.
*
* Handler nhận context lúc `configure()`; đặt sau đó thì các handler đã nạp giữ context cũ.
* Buffer TF là **non-const** có chủ đích: handler dò không chỉ đọc mà còn ghi lại pose đã lọc để
* chặng sau tra — xem `action_core::ActionContext`.
*/
void setContext(const action_core::ActionContext& context);
/**
* @brief Đăng ký một handler dựng sẵn (test, hoặc handler biên dịch thẳng vào host).
* @return false nếu handler null, không khai `actionType` nào, hoặc trùng type đã có chủ.
*/
bool registerHandler(const ActionHandler::Ptr& handler);
bool registerHandler(const action_core::ActionHandler::Ptr& handler);
bool configure(robot::NodeHandle& nh) override;
bool start(const robot_protocol_msgs::Action& action) override;
@@ -74,37 +90,34 @@ public:
/// @brief Số handler đã nạp.
std::size_t handlerCount() const
{
return handlers_.size();
return registry_.size();
}
/// @brief Các `actionType` đã có handler nhận. Dùng cho log và test.
std::vector<std::string> supportedActionTypes() const;
std::vector<std::string> supportedActionTypes() const
{
return registry_.actionTypes();
}
/// @brief Handler nhận @p action_type, hoặc nullptr.
ActionHandler* find(const std::string& action_type) const;
action_core::ActionHandler* find(const std::string& action_type) const
{
return registry_.find(action_type);
}
private:
/// Nạp một handler. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh,
const std::string& ns);
/// Dịch kết quả của framework sang kiểu của port.
static ActionTick toTick(const action_core::ActionTick& tick);
ClockPort* clock_ = nullptr; ///< non-owning
std::string namespace_ = "actions";
action_core::ActionContext context_;
bool configured_ = false;
std::vector<ActionHandler::Ptr> handlers_;
std::map<std::string, ActionHandler*> by_type_; ///< non-owning, trỏ vào handlers_
action_core::ActionRegistry registry_;
ActionHandler* active_ = nullptr; ///< non-owning
action_core::ActionHandler* active_ = nullptr; ///< non-owning, thuộc registry_
std::string active_action_id_;
/**
* Giữ factory của Boost.DLL sống đúng bằng vòng đời runner.
*
* Không phải biến thừa: factory nắm `shared_library` bên trong, thả nó ra là `.so` bị unload
* trong khi handler tạo từ nó vẫn còn sống — vtable trỏ vào vùng đã gỡ.
*/
std::vector<std::function<ActionHandler::Ptr()>> factories_;
};
} // namespace move_base2

34
include/move_base2/runners/controller_runner.h
View File

@@ -20,6 +20,7 @@
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/local_planner.h>
#include <move_base2/io/runtime_stats.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/pose_port.h>
@@ -120,8 +121,21 @@ public:
// ================================================================================================
bool swapPlanner(const std::string& planner_name) override;
void setTolerance(double xy_m, double yaw_rad) override;
bool setDockingMarker(const std::string& marker) override;
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override;
/**
* @brief Dựng lại local planner đang active sau khi footprint local costmap đổi.
*
* Chỉ gọi từ control thread. Không thêm virtual hook vào `robot_nav_core2::LocalPlanner`: các
* plugin được nạp qua Boost.DLL có thể đã biên dịch theo vtable cũ. Dựng lại instance bằng
* factory hiện có giữ ABI nguyên vẹn, khiến `initialize()` đọc lại footprint mới; sau đó plan
* đang chạy được nạp lại để controller không mất chặng giữa đường.
*/
bool refreshActivePlanner();
/// @brief Gắn bộ telemetry (non-owning, có thể null = tắt đo). Gọi trước @ref configure.
void attachStats(RuntimeStats* stats);
bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override;
bool isGoalReached() override;
void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) override;
@@ -159,13 +173,31 @@ private:
const PosePort* pose_ = nullptr; ///< Non-owning.
bool configured_ = false;
/// Telemetry non-owning, null = tắt đo. Chỉ đọc sau khi @ref attachStats.
RuntimeStats* stats_ = nullptr;
RuntimeStats::SectionId section_compute_ = RuntimeStats::kInvalidSection;
RuntimeStats::SectionId section_local_plan_ = RuntimeStats::kInvalidSection;
std::map<std::string, Loaded> controllers_;
std::string active_name_;
robot_nav_core2::LocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_.
/**
* Marker vừa đổi qua @ref setDockingMarker — instance ở lần @ref acquire kế tiếp phải được dựng
* lại từ factory: docking planner đọc `maker_name` đúng MỘT lần trong `initialize()` (getMaker),
* instance cache giữ marker cũ là robot dock vào nhầm trạm. Cờ một-lần thay vì so marker theo
* từng entry vì không biết được plugin nào có đọc `maker_name`; theo trình tự gọi của submit
* (setDockingMarker → swapPlanner) cờ này luôn ứng với đúng planner docking.
*/
bool marker_dirty_ = false;
/// 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;
/// Bản sao plan đã được plugin chấp nhận, dùng để nạp lại sau @ref refreshActivePlanner.
std::vector<robot_geometry_msgs::PoseStamped> active_plan_;
/**
* Trần vận tốc và vận tốc đo được gần nhất.
*

14
include/move_base2/runners/planner_runner.h
View File

@@ -24,6 +24,7 @@
#include <robot/node_handle.h>
#include <robot_nav_core/base_global_planner.h>
#include <move_base2/io/runtime_stats.h>
#include <move_base2/ports/planner_port.h>
namespace robot_costmap_2d
@@ -109,6 +110,14 @@ public:
bool swapPlanner(const std::string& planner_name) override;
/**
* @brief Gắn bộ telemetry (non-owning, có thể null = tắt đo).
*
* Phải gọi TRƯỚC @ref configure: thread lập plan khởi động trong configure() và tự đăng ký nhãn
* của nó ngay khi chạy, nên gắn muộn hơn là thread đó không bao giờ xuất hiện trong bảng.
*/
void attachStats(RuntimeStats* stats);
bool startPlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
const robot_protocol_msgs::Order* order, std::uint64_t tag) override;
@@ -145,6 +154,11 @@ private:
robot::NodeHandle nh_;
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
bool configured_ = false;
/// Telemetry non-owning, null = tắt đo. Chỉ đọc sau khi @ref attachStats.
RuntimeStats* stats_ = nullptr;
RuntimeStats::SectionId section_make_plan_ = RuntimeStats::kInvalidSection;
std::map<std::string, Loaded> planners_;
std::string active_name_;

16
include/move_base2/runners/recovery_runner.h
View File

@@ -92,6 +92,20 @@ public:
*/
bool configure(robot::NodeHandle& nh) override;
/**
* @brief Resolve YAML `<ns>/routes` từ tên behavior sang index của registry.
*
* Gọi sau @ref configure. Schema cũ không có `routes` vẫn hợp lệ: mọi trigger dùng toàn bộ
* behavior đã nạp theo thứ tự registry. Một tên có trong route nhưng plugin chưa nạp được (ví dụ
* `detour_path` đang được phát triển) bị bỏ qua có cảnh báo; route còn ít nhất một behavior vẫn
* chạy an toàn. Nếu sau khi bỏ không còn behavior nào, hàm trả false để chặn runtime khởi động
* với route không thể thực thi.
*/
bool configureRoutes(robot::NodeHandle& nh, std::string& error);
/// @brief Route đã resolve; chỉ hợp lệ sau @ref configureRoutes trả true.
const RecoveryRoutes& routes() const;
std::size_t behaviorCount() const override;
RecoveryOutputKind outputKind(std::size_t index) const override;
bool start(std::size_t index, RecoveryTrigger trigger) override;
@@ -155,6 +169,8 @@ private:
PoseBridge pose_bridge_;
PlanBridge plan_bridge_;
RecoveryRoutes routes_;
bool configured_ = false;
recovery_core::RecoveryBehavior* active_ = nullptr; ///< non-owning, thuộc registry_
};