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

64
CMakeLists.txt
View File

@@ -70,6 +70,14 @@ if(NOT BUILDING_WITH_CATKIN)
${STANDALONE_PACKAGE_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
# These runtime packages are siblings while they still live below Test/.
# The explicit paths keep `cmake ..` in move_base2 working before the
# packages are moved below pnkx_nav_core/src.
${CMAKE_CURRENT_SOURCE_DIR}/../recovery_core/include
${CMAKE_CURRENT_SOURCE_DIR}/../action_core/include
${CMAKE_CURRENT_SOURCE_DIR}/../mission_adapters/include
${CMAKE_CURRENT_SOURCE_DIR}/../nav_test_harness/include
/usr/local/include
)
@@ -83,6 +91,14 @@ if(NOT BUILDING_WITH_CATKIN)
robot_cpp
robot_time
robot_xmlrpcpp
# move_base2 public headers and runners use these packages directly.
# When configured from the pnkx_nav_core root, these names resolve to
# CMake targets and propagate their public include directories.
recovery_core
action_core
mission_adapters
nav_test_harness
)
find_library(TF3_LIBRARY
@@ -91,7 +107,7 @@ if(NOT BUILDING_WITH_CATKIN)
)
if(NOT TF3_LIBRARY)
message(FATAL_ERROR "tf3 không tìm thấy — cài tf3 (/usr/local/lib/libtf3.so) trước khi build")
message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
endif()
if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR})
@@ -127,7 +143,11 @@ else()
# RecoveryRunner include thẳng recovery_core: đó là chỗ duy nhất trong gói này biết tới nó.
recovery_core
# MissionAdapterBridge include thẳng mission_adapters: chỗ duy nhất trong gói này biết tới nó.
# ActionRunner include thẳng action_core: chỗ duy nhất trong gói này biết tới nó.
action_core
# `bridges/` include thẳng mission_adapters: biên duy nhất trong gói này biết tới nó
# (MissionAdapterBridge dịch contract, MissionLayer lắp ráp framework).
mission_adapters
# scenario_test chạy kịch bản khai báo qua khung của nav_test_harness.
@@ -140,7 +160,7 @@ else()
)
if(NOT TF3_LIBRARY)
message(FATAL_ERROR "tf3 không tìm thấy — cài tf3 (/usr/local/lib/libtf3.so) trước khi build")
message(FATAL_ERROR "tf3 not found — install tf3 (/usr/local/lib/libtf3.so) before building")
endif()
catkin_package(
@@ -150,7 +170,6 @@ else()
LIBRARIES
move_base2_core
move_base2
move_base2_noop_action_handler
CATKIN_DEPENDS
move_base_core
@@ -201,9 +220,11 @@ add_library(move_base2_core SHARED
src/runners/action_runner.cpp
src/runners/planner_runner.cpp
src/runners/controller_runner.cpp
src/io/runtime_stats.cpp
src/io/sensor_gateway.cpp
src/io/costmap_exporter.cpp
src/bridges/mission_adapter_bridge.cpp
src/bridges/mission_layer.cpp
src/navigation_runtime.cpp
)
@@ -217,35 +238,6 @@ target_include_directories(move_base2_core
)
# ========================================================
# ActionHandler mặc định — plugin riêng, nạp qua Boost.DLL như mọi handler khác.
# ========================================================
add_library(move_base2_noop_action_handler SHARED
plugins/noop_action_handler.cpp
)
target_compile_options(move_base2_noop_action_handler PRIVATE -Wall -Wextra)
target_include_directories(move_base2_noop_action_handler
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
set_target_properties(move_base2_noop_action_handler PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_link_libraries(move_base2_noop_action_handler
PUBLIC
move_base2_core
PRIVATE
${catkin_LIBRARIES}
Boost::boost
Boost::system
Boost::filesystem
${CMAKE_DL_LIBS}
)
# ========================================================
# Plugin library — facade BaseNavigation + export Boost.DLL.
# ========================================================
@@ -357,7 +349,7 @@ endif()
# ========================================================
if(BUILDING_WITH_CATKIN)
install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler
install(TARGETS move_base2_core move_base2
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
@@ -374,7 +366,7 @@ if(BUILDING_WITH_CATKIN)
else()
install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler
install(TARGETS move_base2_core move_base2
EXPORT ${PROJECT_NAME}-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
@@ -442,10 +434,12 @@ if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN)
action_runner_test
recovery_runner_test
sensor_gateway_test
runtime_stats_test
navigation_server_test
planner_runner_test
controller_runner_test
mission_adapter_bridge_test
mission_layer_test
move_base2_scenario_test # tên có tiền tố gói: nav_test_harness đã có target scenario_test
)

17
README.md
View File

@@ -79,6 +79,19 @@ cấu hình đang có hiệu lực sẽ nằm trong cây config của nav core,
## Trạng thái
Bộ khung đã chạy được end-to-end với thành phần giả. Phần chưa có — hiện thực port thật, thread
planner, đẩy sensor vào costmap, lớp nối tới mission và recovery framework — được liệt kê ở cuối
Runtime thật đã chạy trọn trên sim: costmap, planner/controller/recovery/action nạp qua boost::dll,
và mission layer (`mission_adapters`) được dựng trong `NavigationRuntime` nên order VDA5050 được cắt
thành từng chặng thay vì dồn thành một goal. Phần còn thiếu được liệt kê ở cuối
`docs/ARCHITECTURE.md`.
### Order VDA5050 đi đường nào
`NavigationServer::moveTo(Order, …)` thử `MissionLayer::submitOrder()` trước. Layer nhận thì order đi
qua `mission_adapters`: cắt chặng tại mỗi node có action, chỉ chạy phần `released`, `orderUpdateId`
nối tiếp thay vì chạy lại, `mission_timeout` làm lưới cuối. Layer từ chối (tắt bằng config, hoặc
không nạp được nguồn nào cho schema `vda5050.order`) thì order rơi xuống đường trực tiếp — một
`NavigationRequest` cho cả order, đúng hành vi gen-1.
Sáu entry point còn lại (`moveTo(goal)`, `dockTo`, `moveStraightTo`, `rotateTo`) **không** đi qua
mission layer: chúng mang theo sai số hình học và motion profile riêng, mà mission layer không có
chỗ chứa hai thứ đó.

77
config/runtime/action_handlers_params.yaml Normal file
View File

@@ -0,0 +1,77 @@
# Action handler của move_base2 (D8: navigation runtime chạy trọn một mission — nav xong thì chạy
# nốt action của chặng rồi mới báo kết quả).
#
# Handler là plugin của gói `action_core`, nạp qua Boost.DLL đúng như planner/recovery. Thêm một
# loại action mới = viết một `.so` + thêm một dòng ở đây, không phải sửa dòng nào của navigation.
#
# Vì sao file này tồn tại: thiếu handler thì `ActionRunner::start` từ chối và chặng mang action đi
# thẳng tới ABORTED — tức mọi order VDA5050 có node action fail ở chặng đầu tiên.
actions:
handlers:
# Dò: lấy mẫu frame thô của LiDAR/camera, lọc, ghi ra `dock_target` cho chặng sau tra.
- {name: detect, type: FrameSamplerActionHandler}
# Hai handler THẬT, chạy được cả trên robot: chúng không điều khiển thiết bị nào vì chính action
# đó không yêu cầu thiết bị nào.
- {name: waiter, type: WaitActionHandler}
- {name: reporter, type: LogReportActionHandler}
# ⚠ STUB MÔ PHỎNG, CHỈ DÀNH CHO SIM/DEV. `NoopActionHandler` chỉ log rồi báo thành công sau
# `duration` giây — nó KHÔNG nói chuyện với thiết bị nào. Trên robot thật, một `PickUp` chạy qua
# đây nghĩa là robot báo "đã nâng kệ" trong khi càng nâng chưa hề nhúc nhích, và fleet master sẽ
# giao chặng tiếp theo với giả định hàng đã ở trên xe. Thay bằng handler thật (nói chuyện với
# PLC/băng tải/càng nâng) trước khi chạy ngoài hiện trường.
- {name: sim_noop, type: NoopActionHandler}
detect:
action_types: [DetectCharger, DetectPallet]
output_frame: dock_target # chặng docking trỏ `move_to` vào đây
# STUB chỉ cho Gazebo/dev: thay perception bằng target cố định (-1.5, +0.2) trong `base_link`,
# theo đúng pose `trolley_goal` giả trước đây. Handler quy nó sang `map` MỘT LẦN rồi ghi
# `dock_target` tĩnh vào tf3, nên target không chạy theo robot khi chặng docking bắt đầu.
# PHẢI tắt trước khi chạy robot thật để handler lấy frame do perception publish.
use_simulated_goal: true
simulated_parent_frame: base_link
simulated_offset_x: 1.5 # [m]
simulated_offset_y: 0.2 # [m]
simulated_offset_yaw: 0.0 # [rad]
settle_delay: 1.0 # [s] chờ robot đứng hẳn — pose lúc vừa dừng còn dao động cơ khí
sample_window: 2.0 # [s] thu mẫu
min_samples: 20
max_spread_xy: 0.03 # [m] tản hơn -> kFailed, KHÔNG lùi vào
max_spread_yaw: 0.05 # [rad]
timeout: 15.0 # [s] frame thô không tới -> kFailed
waiter:
action_types: [wait]
default_duration: 2.0 # [s] dùng khi action không kèm tham số `duration`
max_duration: 600.0 # [s] trần cứng; vượt trần thì action THẤT BẠI, không bị cắt ngắn
reporter:
action_types: [logReport]
sim_noop:
# ⚠ `PickUp` và `charge` KHÔNG có ở đây: chúng là compound action, adapter đã dịch chúng thành
# chuỗi chặng và GỠ khỏi danh sách action. Thứ tới được ActionRunner là các actionType do chuỗi
# đó sinh ra (`LiftFork`, `startCharging`) cộng với action thường của order.
#
# Đây đúng chỗ dễ trôi lệch nhất giữa hai bảng: khai `PickUp` ở đây thì handler không bao giờ
# được gọi, còn quên `LiftFork` thì chặng thiết bị ABORT giữa chừng với kệ đang trên càng.
action_types: [LiftFork, startCharging, DropDown, Drop, MutedOn, MutedOff, pick, drop]
duration: 2.0 # [s] thời gian giả lập thiết bị làm việc
timeout: 30.0 # [s] timeout TẦNG 1 — trách nhiệm của chính handler
# Bảng symbol -> thư viện cho Boost.DLL. Thiếu `library_path` là nguyên nhân phổ biến nhất của lỗi
# "plugin build xong nhưng runtime báo không tìm thấy".
FrameSamplerActionHandler:
library_path: libaction_core_frame_sampler_action_handler
WaitActionHandler:
library_path: libaction_core_wait_action_handler
LogReportActionHandler:
library_path: libaction_core_log_report_action_handler
NoopActionHandler:
library_path: libaction_core_noop_action_handler

54
config/runtime/compound_actions_params.yaml Normal file
View File

@@ -0,0 +1,54 @@
# Action "phải dò rồi mới biết đích" — bảng mở rộng của `VDA5050SourceAdapter`.
#
# File riêng thay vì sửa `mission_adapters_params.yaml`: file đó là symlink sang cây config của
# `pnkx_nav_core` (submodule). `robot::NodeHandle` gộp mọi file YAML trong thư mục config vào một
# cây, nên khoá `vda5050_src` ở đây hợp nhất với khoá cùng tên bên kia.
#
# ── Bảng này chỉ giữ CẤU TRÚC ────────────────────────────────────────────────────────────────────
#
# Fleet chỉ gửi `charge`; `DetectCharger` và `startCharging` là actionType NỘI BỘ do adapter sinh ra,
# không bao giờ xuất hiện trong order JSON. Tên frame cụ thể đến từ `actionParameters` của chính
# order, nên thêm một trạm sạc mới không phải sửa file này.
#
# Bốn từ khoá của một step, phải có ĐÚNG MỘT:
# action: <type> chặng chỉ-action; mang nguyên actionParameters của action gốc
# move_to: <frame> chặng nav, frame cố định
# move_to_param: <key> chặng nav, frame lấy từ actionParameters[<key>] của action gốc
# move: <mét> chặng nav tương đối; dương = tiến, âm = lùi
# Tuỳ chọn cho step navigation: profile (position | docking | go_straight | rotate), marker.
# `marker` chỉ hợp lệ với `profile: docking`: nó chọn override trong
# maker_sources.yaml/docking_marker_profiles. Không khai = dùng cặp docking mặc định.
# Tolerance thuộc YAML riêng của local planner đang được profile chọn, không khai ở đây.
vda5050_src:
compound_actions:
charge:
steps:
# 1. Dò: đứng yên lấy mẫu frame thô, lọc nhiễu, ghi ra `dock_target`.
# Robot không phát cmd_vel trong EXECUTING_ACTIONS nên đây đúng là lúc để lọc.
- {action: DetectCharger}
# 2. Tiến vào đích vừa dò được bằng profile docking. Sai số do local planner đọc từ YAML
# của chính plugin đó.
- {move_to: dock_target, profile: docking, marker: charger}
# 3. Thao tác thiết bị. Chặng 1 hoặc 2 hỏng thì `clear_queue_on_failure` xoá chặng này —
# robot không bao giờ đóng relay khi chưa vào được vị trí.
- {action: startCharging}
PickUp:
steps:
- {action: DetectPallet}
- {move_to: dock_target, profile: docking, marker: trolley}
- {action: LiftFork}
# Lùi ra khỏi khe kệ. Quãng đường tương đối được quy ra pose tuyệt đối tại `submit`, từ pose
# LÚC ĐÓ — lúc adapter sinh chặng thì robot còn chưa tới nơi.
- {move: -0.5, profile: go_straight}
GoStraight:
steps:
- {move: 1.0, profile: go_straight}
Rotate:
steps:
- {move: 0.0, profile: rotate}

1
config/runtime/maker_sources.yaml
View File

@@ -1 +0,0 @@
../../../../pnkx_nav_core/config/maker_sources.yaml

23
config/runtime/maker_sources.yaml Normal file
View File

@@ -0,0 +1,23 @@
# Compatibility allow-list for BaseNavigation::dockTo(marker, ...).
#
# move_base2 docking resolves `goal_frame` to an absolute pose and runs DockPlanner +
# HybridLocalPlanner. It does not consume the legacy per-marker PNKXDockingLocalPlanner
# parameters (plugins, maker_goal_frame, delay, timeout, velocity, tolerance, or lookahead).
# Keep only the marker names that the host may submit until `dockTo` is migrated to
# declarative dock_sequences.
maker_sources: trolley charger dock_station undock_station dock_station_2 undock_station_2
# Marker có entry ở đây dùng cặp planner riêng. Marker rỗng hoặc chỉ có trong `maker_sources` mà
# không nằm trong bảng này sẽ dùng cặp `docking` mặc định của move_base_common_params.yaml.
docking_marker_profiles:
trolley:
global_planner: DockPlanner
local_planner: HybridLocalPlanner
charger:
global_planner: DockPlanner
local_planner: HybridLocalPlanner
dock_station:
global_planner: DockPlanner
local_planner: HybridLocalPlanner

98
config/runtime/move_base_common_params.yaml
View File

@@ -1,60 +1,74 @@
position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
base_global_planner: SBPLLatticePlanner
# Cặp global/local planner của từng kiểu chuyển động. move_base2 nạp trực tiếp
# `robot_nav_core2::LocalPlanner`; không dùng LocalPlannerAdapter (bridge chỉ dành cho move_base cũ).
position:
global_planner: CustomPlanner
local_planner: HybridLocalPlanner
PriestLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: SBPLLatticePlanner #CustomPlanner SBPLLatticePlanner
docking:
# goal_frame đã được ControlLoop quy thành pose tuyệt đối. DockPlanner dùng đúng contract
# makePlan(start, goal, plan), còn CustomPlanner chỉ xử lý makePlan(Order, start, goal, plan).
global_planner: DockPlanner
local_planner: HybridLocalPlanner
PNKXDockingLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
go_straight:
global_planner: TwoPointsPlanner
local_planner: PNKXGoStraightLocalPlanner
PNKXGoStraightLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
rotate:
global_planner: TwoPointsPlanner
local_planner: PNKXRotateLocalPlanner
PNKXRotateLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
# Đường lùi chung cho mọi profile: planner chính trả false hoặc plan rỗng thì move_base2 đổi sang
# planner này đúng MỘT lần cho request hiện tại. Backup cũng fail thì mới chạy recovery; request mới
# luôn bắt đầu lại từ planner chính. Backup gọi makePlan(start, goal, plan), không mang VDA5050
# Order, để SBPLLatticePlanner (chỉ có overload ba tham số) dùng được.
backup_global_planner: SBPLLatticePlanner
# Compound docking quy `goal_frame` thành pose tuyệt đối; HybridLocalPlanner không đọc maker_name.
# `true` chỉ dành cho PNKXDockingLocalPlanner legacy, vốn phải chọn marker trước initialize().
docking_requires_marker: false
# Bảng `library_path` và tham số riêng của CustomPlanner, DockPlanner, TwoPointsPlanner cùng các
# local planner nằm trong các YAML runtime đồng hành (symlink từ pnkx_nav_core/config/). Không lặp
# lại chúng ở đây để tránh hai nguồn cấu hình cho cùng một plugin.
### replanning
controller_frequency: 30.0 # run controller at 30.0 Hz
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
controller_patience: 0.033333333 # [s] giữ hành vi cũ: fail controller -> recovery sau một cycle 30 Hz
planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 0 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.5
## recovery behaviors
### telemetry
#
# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở
# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá
# `recovery_behaviors` ở đây.
# [s] Chu kỳ in bảng thông số runtime ra terminal: CPU của từng thread đã đăng ký (control loop,
# thread lập plan, hai vòng cập nhật costmap), chi phí từng đoạn công việc (local planner, global
# planner, cachePlans), RSS và tốc độ tăng RSS. 0 = tắt hẳn, không đo gì.
#
# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` /
# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1
# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên
# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file
# config tranh nhau cùng một tên alias.
recovery_behavior_enabled: false
recovery_behaviors: [
{name: aggressive_reset, type: ClearCostmapRecovery},
{name: conservative_reset, type: ClearCostmapRecovery},
]
# Dòng "(không đăng ký)" trong bảng là phần CPU KHÔNG thuộc navigation stack — nó thuộc host ROS
# (callback cảm biến, OPC-UA, VDA5050, TF bridge). Đọc con số đó trước khi kết luận move_base2 nặng.
#
# Đây là công cụ chẩn đoán: tắt lại khi đã đo xong, đừng để chạy thường trực trên robot thật.
runtime_stats_period: 0.0
# Recovery gen-2 nằm ở `recovery_behaviors_params.yaml`, namespace `recovery`.
recovery_behavior_enabled: true
conservative_reset:
reset_distance: 3.0 # clear obstacles farther away than 3.0 m
invert_area_to_clear: true
aggressive_reset:
reset_distance: 3.0
ClearCostmapRecovery:
library_path: librobot_clear_costmap_recovery
## mission layer
#
# true (mặc định của move_base2): VDA5050 Order đi qua `mission_adapters` — 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 từ đầu, và có `mission_timeout` làm lưới cuối. Nguồn mission và tham số của layer khai ở
# `mission_adapters_params.yaml`.
#
# false: order đi thẳng xuống navigation như MỘT goal duy nhất — hành vi của move_base gen-1, và
# cũng là hành vi đã chạy được trên sim trước 2026-07-31. Đây là đường lùi khi mission layer gây vấn
# đề trên hiện trường: đổi một khoá, không phải build lại.
#
# 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.
mission_layer_enabled: true
MoveBase:
library_path: libmove_base2

50
docs/ARCHITECTURE.md
View File

@@ -50,14 +50,16 @@ lẫn nhau. Hệ quả có thật, không phải hình thức:
- `move_base2` không bị khoá cứng vào một hiện thực mission hay recovery cụ thể — đổi framework chỉ
cần viết lại lớp nối, không phải sửa lõi.
Ở Phase 1, chiều này còn được giữ ở mức mạnh hơn: **không file nào trong `move_base2` include hai
framework kia**. Lớp nối (`MissionAdapterBridge`, `RecoveryRunner`) được thêm ở bước sau, và chúng
mới là chỗ duy nhất được phép include.
Ở Phase 1, chiều này còn được giữ ở mức mạnh hơn: **không file nào trong `move_base2` include ba
framework kia**. Lớp nối được thêm ở bước sau và chúng mới là chỗ duy nhất được phép include:
`RecoveryRunner` cho `recovery_core`; `ActionRunner` cho `action_core`;
`bridges/mission_adapter_bridge` (dịch contract) và `bridges/mission_layer` (lắp ráp registry +
hàng đợi + hai thread) cho `mission_adapters`.
Kiểm bằng:
```bash
grep -rn "mission_adapters\|recovery_core" src/AMR_T800/Test/move_base2/include \
grep -rn "mission_adapters\|recovery_core\|action_core" src/AMR_T800/Test/move_base2/include \
src/AMR_T800/Test/move_base2/src
```
@@ -77,9 +79,13 @@ Bảy port đều nhỏ và đều tồn tại vì một lý do vận hành cụ
## Quyết định thiết kế đáng ghi lại
**Sáu entry point gộp thành một.** `moveTo` ×2, `dockTo` ×2, `moveStraightTo`, `rotateTo` của
contract host chỉ khác nhau ở kiểu chuyển động và sai số mặc định. Bảng `ProfileBinding` mô tả đúng
phần khác nhau đó; sáu hàm còn lại chỉ dựng struct rồi gọi một đường vào duy nhất.
**Sáu entry point gộp về một contract lõi.** `moveTo` ×2, `dockTo` ×2, `moveStraightTo`, `rotateTo`
của contract host chỉ khác nhau ở kiểu chuyển động. `moveTo(PoseStamped)` là direct **position**
goal nên trước tiên đi `MissionLayer::submitGoal()` → `GoalSourceAdapter` → `MissionManager`; nhờ đó
nó nhận mission ID và lifecycle/cancel giống VDA5050. Nếu mission layer bị tắt hoặc không nạp source
này mới fallback tương thích về `NavigationRequest` trực tiếp. Các API mang profile/marker riêng
(`dockTo`, `moveStraightTo`, `rotateTo`) dựng request trực tiếp, vì schema `geometry.pose_stamped`
chưa biểu diễn được marker/profile của chúng.
**Một đường vào duy nhất.** `ControlLoop::submit()` là chỗ duy nhất một goal lọt được vào lõi, và
`IDLE → PLANNING` là transition duy nhất bắt đầu một chặng. Nhờ đó việc chống hai nguồn goal tranh
@@ -109,14 +115,24 @@ Việc giảm tốc theo động học thuộc về bộ điều khiển bánh x
- `getTwist()` trả **lệnh** vận tốc từ `VelocityArbiter`, đóng dấu theo đồng hồ của control loop.
- Plugin `libmove_base2.so` export alias `MoveBase2`.
Chưa có, thuộc bước nối dây runtime:
Bước nối dây runtime đã xong (Phase 4):
- Hiện thực thật của `PlannerPort` / `ControllerPort` / `PosePort`
(bọc costmap, boost::dll, TF).
- Dựng hai `Costmap2DROBOT` thật. Hiện `NavigationServer::attachCostmaps()` nhận `LayeredCostmap*`
từ bên ngoài bơm vào — cố ý, để đường cảm biến kiểm được mà không cần TF và cây config thật.
- Thread planner riêng và bộ đệm plan ba lớp. Hiện `ControlLoop` lập plan đồng bộ ngay trong cycle;
tách như vậy để phần quyết định kiểm được mà không cần thread.
- Lớp nối tới mission framework.
- `setTwistLinear` / `setTwistAngular` (hiện trả `false` để host biết lệnh không có hiệu lực, thay vì
âm thầm bỏ qua).
- `PlannerRunner` / `ControllerRunner` / `RecoveryRunner` / `ActionRunner` nạp plugin thật qua
boost::dll; `CostmapPosePort` lấy pose từ costmap — **hai** instance, khác frame (`map` cho
planner, `odom` cho controller/recovery).
- `NavigationRuntime` dựng hai `Costmap2DROBOT` thật rồi trả về `ControlLoopDeps`.
`NavigationServer::attachCostmaps()` vẫn nhận `LayeredCostmap*` từ ngoài để đường cảm biến kiểm
được mà không cần TF và cây config thật.
- Thread planner riêng + hoán vị ba buffer, `PlannerPort` bất đồng bộ.
- `setTwistLinear` / `setTwistAngular` — **trần vận tốc**, không phải lệnh jog.
- Lớp nối tới mission framework: `MissionAdapterBridge` (dịch contract) + `MissionLayer` (dựng
registry, hàng đợi, hai thread). Order VDA5050 vào bằng `moveTo(Order, …)` được cắt thành từng
chặng. Direct position goal vào bằng `moveTo(PoseStamped)` đi `GoalSourceAdapter`, vì vậy cũng có
mission ID thay vì `0`; tắt bằng `mission_layer_enabled: false` thì cả hai loại fallback xuống
đường direct tương thích.
Chưa có:
- Kết xuất lưới costmap cho rviz đã có, nhưng đường `OccupancyGridUpdate` incremental đã bị bỏ —
luôn gửi lưới đầy đủ ở 1 Hz.
- Action chạy **dọc đường đi** (edge action): mọi action hiện chạy sau khi tới goal của chặng.

184
docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html Normal file
View File

@@ -0,0 +1,184 @@
<!DOCTYPE html>
<html lang="vi">
<head>
<meta charset="UTF-8">
<title>move_base2: kiến trúc, C API và tương thích</title>
<style>
@page { size: A4; margin: 1.6cm 1.65cm 1.55cm 1.65cm; }
body { font-family: "Liberation Sans", "DejaVu Sans", sans-serif; color: #18212b; font-size: 10.5pt; line-height: 1.42; }
h1 { color: #0b4f71; font-size: 22pt; margin: 0 0 4px; }
h2 { color: #0b4f71; font-size: 15pt; border-bottom: 1px solid #b9d4e3; padding-bottom: 3px; margin: 22px 0 8px; }
h3 { color: #215e7a; font-size: 11.7pt; margin: 14px 0 5px; }
p { margin: 6px 0; }
ul, ol { margin: 5px 0 8px 23px; padding: 0; }
li { margin: 3px 0; }
.subtitle { color: #536675; font-size: 11pt; margin-bottom: 18px; }
.answer { background: #eaf6ed; border-left: 5px solid #31965a; padding: 10px 13px; margin: 13px 0; }
.warning { background: #fff5dd; border-left: 5px solid #dc9a22; padding: 9px 12px; margin: 11px 0; }
.note { background: #eaf3f8; border-left: 5px solid #287da9; padding: 9px 12px; margin: 11px 0; }
table { width: 100%; border-collapse: collapse; margin: 8px 0 12px; font-size: 9.5pt; }
th { background: #0b4f71; color: white; text-align: left; padding: 6px; }
td { border: 1px solid #c9d4da; padding: 6px; vertical-align: top; }
tr:nth-child(even) td { background: #f6f9fa; }
code { font-family: "Liberation Mono", "DejaVu Sans Mono", monospace; font-size: 8.8pt; color: #7a2f16; }
pre { background: #f1f4f6; border: 1px solid #d7e0e4; padding: 8px; white-space: pre-wrap; font-family: "Liberation Mono", "DejaVu Sans Mono", monospace; font-size: 8.3pt; line-height: 1.33; }
.small { font-size: 8.7pt; color: #52616b; }
.flow { border: 1px solid #bdd5e2; background: #f7fbfd; padding: 10px; text-align: center; font-weight: bold; color: #215e7a; }
.page-break { page-break-before: always; }
</style>
</head>
<body>
<h1>move_base2: kiến trúc, C API và tương thích</h1>
<p class="subtitle">Phân tích theo mã nguồn và log chạy của T800 &middot; 03/08/2026</p>
<div class="answer">
<strong>Kết luận ngắn.</strong> Khung chương trình T800 đang nạp <code>move_base</code> qua interface
<code>robot::move_base_core::BaseNavigation</code> và factory alias <code>"MoveBase"</code> có thể chạy
<code>move_base2</code> mà không phải sửa host, nếu đổi đúng thư viện và cây cấu hình runtime.
Điều này đúng cho khung T800 hiện tại; không phải quy tắc tự động đúng cho mọi ứng dụng ROS dùng package
<code>move_base</code>.
</div>
<h2>1. Khung đang khởi tạo những gì?</h2>
<p>Log cho thấy chỉ có một node ROS là <code>/amr_node</code> cho phần điều khiển. Bên trong process đó,
<code>amr_control</code> nạp động navigation runtime và các plugin. Global planner, local planner, action,
recovery và mission adapter <strong>không</strong> là node ROS riêng.</p>
<div class="flow">
roslaunch &rarr; /amr_node (amr_control) &rarr; BaseNavigation factory &rarr; NavigationServer (move_base2)<br>
&rarr; costmap global/local + runners + mission/action/recovery plugins
</div>
<table>
<tr><th>Thành phần</th><th>Thời điểm / cách tạo</th><th>Vai trò quan sát được</th></tr>
<tr><td><code>amr_control</code></td><td>Node <code>/amr_node</code>; tạo TF, localization, sensor converter, publisher/subscriber rồi nạp navigation.</td><td>Cầu nối ROS/MQTT/OPC-UA với lõi navigation.</td></tr>
<tr><td><code>NavigationServer</code></td><td>Factory của <code>libmove_base2.so</code> trả về <code>BaseNavigation::Ptr</code>.</td><td>Vỏ tương thích <code>BaseNavigation</code>; quản lý runtime và control thread 30 Hz.</td></tr>
<tr><td>Global/local costmap</td><td>Tạo khi <code>NavigationRuntime::buildCostmaps()</code> chạy.</td><td>Global dùng frame <code>map</code>; local dùng <code>odom</code>; nhận laser, cloud, depth qua SensorGateway.</td></tr>
<tr><td>PlannerRunner</td><td>Nạp <code>CustomPlanner</code> lúc boot; nạp <code>SBPLLatticePlanner</code>, <code>DockPlanner</code> khi profile cần.</td><td>Lập global plan, cache instance theo tên planner.</td></tr>
<tr><td>ControllerRunner</td><td>Nạp <code>HybridLocalPlanner</code> lúc boot; có thể đổi theo profile.</td><td>Biến plan thành lệnh vận tốc.</td></tr>
<tr><td>RecoveryRunner</td><td>Nạp lúc boot từ namespace <code>recovery</code>.</td><td>Trong log: wait, clear-costmap (2 mức), rotate, back-up.</td></tr>
<tr><td>ActionRunner</td><td>Nạp lúc boot từ namespace <code>actions</code>.</td><td>Trong log: detect, wait, report, sim_noop.</td></tr>
<tr><td>Mission layer</td><td>Nạp source adapter lúc boot từ <code>mission_adapters</code>.</td><td><code>GoalSourceAdapter</code> nhận goal; <code>VDA5050SourceAdapter</code> tách order thành các leg/nav/action.</td></tr>
</table>
<h3>Luồng một VDA5050 order</h3>
<ol>
<li>MQTT nhận topic order.</li>
<li><code>VDA5050SourceAdapter</code> đổi order thành các mission leg: navigation hoặc action-only.</li>
<li><code>MissionManager</code> đưa leg vào hàng đợi; <code>MissionExecutor</code> chạy tuần tự.</li>
<li>Control loop chọn profile: position, docking, go_straight hoặc rotate; runner lấy planner/local planner phù hợp.</li>
<li>Kết quả nav/action trả về mission layer, rồi trạng thái VDA5050.</li>
</ol>
<h2>2. C API đang dùng gì và có dùng được move_base2 không?</h2>
<p>C API nằm tại <code>pnkx_nav_core/src/APIs/c_api</code>. Nó không tạo trực tiếp lớp C++
<code>move_base::MoveBase</code> hay <code>move_base2::NavigationServer</code>. Hàm
<code>navigation_create()</code> làm đúng chuỗi sau:</p>
<pre>PluginLoaderHelper::findLibraryPath("MoveBase")
boost::dll::import_alias&lt;BaseNavigation::Ptr()&gt;(path, "MoveBase")
factory() -&gt; NavigationHandle</pre>
<p>Vì <code>libmove_base2.so</code> export cả hai alias <code>MoveBase2</code> và <code>MoveBase</code>,
C API sẽ nhận được một <code>NavigationServer</code> của move_base2 khi cấu hình <code>MoveBase</code> trỏ đến
<code>libmove_base2</code>. C API phía gọi không cần đổi tên hàm.</p>
<table>
<tr><th>Nhóm C API</th><th>Ví dụ</th><th>Khi dùng move_base2</th></tr>
<tr><td>Vòng đời</td><td><code>navigation_create</code>, <code>navigation_initialize</code>, <code>navigation_destroy</code></td><td>Dùng được qua interface chung. Nên bảo đảm host gọi <code>shutdown()</code> ở đường C++ trước khi dỡ process/plugin.</td></tr>
<tr><td>Lệnh điều hướng</td><td><code>navigation_move_to</code>, <code>navigation_move_to_order</code>, <code>navigation_dock_to</code>, pause/resume/cancel</td><td>Dùng được. Order/docking được move_base2 đưa vào mission/profile tương ứng.</td></tr>
<tr><td>Sensor và map</td><td><code>navigation_add_static_map</code>, <code>navigation_add_laser_scan</code>, <code>navigation_add_point_cloud2</code>, odometry</td><td>Dùng được; tên nguồn phải khớp source trong costmap YAML, ví dụ <code>pc_r_marking</code>.</td></tr>
<tr><td>Quan sát</td><td><code>navigation_get_feedback</code>, pose/twist, global/local planner data</td><td>Dùng được cho trạng thái <code>BaseNavigation</code>. Không tự lộ API chi tiết của MissionManager hay ActionRunner.</td></tr>
</table>
<div class="warning">
<strong>Lưu ý kỹ thuật C API:</strong> đây là ABI C-linkage (hàm dùng <code>extern "C"</code>), nhưng
header hiện có một số tham số tham chiếu C++ như <code>PoseStamped &amp;out_pose</code> và
<code>size_t &amp;out_count</code>. Vì vậy nó <strong>chưa là header ISO C thuần</strong> để biên dịch trực tiếp
bằng C compiler. Điều này không cản trở việc chọn move_base2, nhưng nếu caller là C thuần thì cần đổi các
output reference thành pointer trước khi coi API là C API hoàn chỉnh.
</div>
<h2>3. move_base2 khác move_base ở đâu?</h2>
<table>
<tr><th>Chủ đề</th><th>move_base cũ trong T800</th><th>move_base2</th></tr>
<tr><td>Ranh giới với host</td><td><code>BaseNavigation</code>, plugin được nạp qua alias <code>MoveBase</code>.</td><td>Giữ cùng interface và xuất alias tương thích <code>MoveBase</code>; thêm alias rõ ràng <code>MoveBase2</code>.</td></tr>
<tr><td>Tổ chức runtime</td><td>Điều phối kiểu đơn khối hơn.</td><td>Tách NavigationRuntime, ControlLoop, PlannerRunner, ControllerRunner, RecoveryRunner, ActionRunner và MissionLayer.</td></tr>
<tr><td>Nhiệm vụ / order</td><td>Host hoặc lớp ngoài thường phải tự điều phối nhiều bước.</td><td>Mission layer có adapter nguồn và executor; VDA5050 order được tách thành leg, action-only leg chạy qua ActionRunner.</td></tr>
<tr><td>Profile</td><td>Thường chỉ một bộ planner/controller cho một kiểu goal.</td><td>Profile position, docking, go_straight, rotate; docking có marker profile. Planner/local planner có thể chuyển theo mission.</td></tr>
<tr><td>Fallback</td><td>Phụ thuộc implementation cũ.</td><td>Log chứng minh: khi CustomPlanner không hỗ trợ request đơn giản, runtime chuyển một lần sang SBPLLatticePlanner.</td></tr>
<tr><td>Safety</td><td>Tùy implementation.</td><td>Có điều kiện <code>require_current_costmap</code>; sensor stale sẽ chặn wheel command để không điều khiển theo thế giới cũ.</td></tr>
<tr><td>Local planner plugin</td><td>Không nên giả định plugin cũ có cùng ABI.</td><td>Dùng contract <code>robot_nav_core2::LocalPlanner</code>; local planner cũ chỉ dùng lại khi đã xác nhận cùng interface/ABI hoặc có adapter.</td></tr>
</table>
<div class="note">
<strong>Đừng nhầm hai mức tương thích.</strong> Host/API <code>BaseNavigation</code> tương thích là một việc.
Plugin local planner, YAML, và hành vi mission/profile tương thích là các việc khác. Việc đổi thư viện thành
công không chứng minh tất cả planner cũ sẽ chạy được trong move_base2.
</div>
<div class="page-break"></div>
<h2>4. Một khung đang dùng move_base có chạy move_base2 luôn không?</h2>
<p><strong>Câu trả lời:</strong> có điều kiện. Với <code>amr_control</code> của T800 thì câu trả lời là
<strong>có, theo đúng cơ chế đã được thiết kế</strong>. Với một khung ROS bất kỳ đang dùng package ROS1
<code>move_base</code>, câu trả lời là <strong>không thể kết luận là có</strong> nếu chưa kiểm tra interface.</p>
<table>
<tr><th>Loại khung hiện có</th><th>Khả năng chuyển</th><th>Lý do / việc cần làm</th></tr>
<tr><td>T800 host nạp <code>BaseNavigation::Ptr</code> từ alias <code>MoveBase</code></td><td><strong>Cao</strong></td><td>move_base2 export alias tương thích. Đặt đúng library/config, rồi kiểm thử runtime.</td></tr>
<tr><td>Ứng dụng gọi C API <code>navigation_create()</code></td><td><strong>Cao</strong></td><td>C API cũng tìm <code>MoveBase</code>; config quyết định .so được nạp. Cần cùng ABI của <code>move_base_core</code>.</td></tr>
<tr><td>Ứng dụng liên kết trực tiếp class/private header của move_base cũ</td><td><strong>Thấp</strong></td><td>Phải sửa và build lại theo interface công khai hoặc làm adapter; không nên thay .so mù quáng.</td></tr>
<tr><td>ROS1 chuẩn dùng action <code>move_base_msgs/MoveBaseAction</code> và pluginlib/nav_core</td><td><strong>Chưa khẳng định</strong></td><td>Đây không phải tự động là contract T800 <code>BaseNavigation</code>; cần kiểm tra node/action/topic/plugin ABI cụ thể.</td></tr>
</table>
<h3>Điều kiện bắt buộc để chuyển khung T800</h3>
<ol>
<li><strong>Chung ABI:</strong> host, <code>libmove_base2.so</code> và <code>move_base_core</code> phải được build từ cùng workspace/devel hoặc ABI tương thích.</li>
<li><strong>Đúng alias:</strong> library phải export factory <code>BaseNavigation::Ptr()</code> dưới tên <code>MoveBase</code>. move_base2 hiện đã có alias này.</li>
<li><strong>Đúng config:</strong> <code>PNKX_NAV_CORE_CONFIG_DIR</code> phải trỏ đến <code>move_base2/config/runtime</code>; file <code>move_base_common_params.yaml</code> cần có <code>MoveBase: library_path: libmove_base2</code>.</li>
<li><strong>Đủ plugin:</strong> global planner/local planner/recovery/action/mission adapter được khai trong YAML phải có .so, alias factory và dependency đúng.</li>
<li><strong>Đúng sensor contract:</strong> map, TF, odom, laser/cloud/depth được bơm bằng đúng topic-key và tần số. Với <code>require_current_costmap: true</code>, sensor stale sẽ chặn lệnh bánh xe.</li>
<li><strong>Shutdown rõ ràng:</strong> dừng navigation trước khi destroy loader/process để tránh race thread/plugin.</li>
</ol>
<h3>Cách chuyển an toàn trong launch của T800</h3>
<pre>&lt;!-- move_base2_control.launch đã làm hai việc quan trọng --&gt;
&lt;env name="PNKX_NAV_CORE_CONFIG_DIR"
value="$(find move_base2)/config/runtime" /&gt;
# config/runtime/move_base_common_params.yaml
MoveBase:
library_path: libmove_base2</pre>
<p>Không đổi trực tiếp link library trong <code>amr_control</code>. Nó tiếp tục nạp factory
<code>"MoveBase"</code>; file cấu hình chọn implementation là move_base2.</p>
<h2>5. Checklist test trước khi thay cho hệ chạy dài</h2>
<ol>
<li>Build: xác nhận có <code>devel/lib/libmove_base2.so</code> và các plugin runtime.</li>
<li>Boot: log phải có <code>Found library ... libmove_base2.so</code>, <code>NavigationRuntime built</code>, costmap và sensor source tạo thành công.</li>
<li>Goal đơn: position, cancel/preempt, pause/resume; xác nhận <code>cmd_vel</code> và feedback.</li>
<li>Order: VDA5050 order có nhiều node/edge, action wait/detect/charge và docking marker.</li>
<li>Fallback/recovery: tạo tình huống CustomPlanner fail, obstacle/stale sensor có kiểm soát; xác nhận fallback/recovery và robot dừng an toàn.</li>
<li>Soak test: chạy 8 giờ, theo dõi CPU/RAM, tần số camera/cloud, message queue Gazebo, reconnect MQTT và tần suất sensor stale.</li>
</ol>
<h2>6. Liên hệ với cảnh báo qua đêm trong log</h2>
<p>Cảnh báo <code>/gazebo/default/pose/local/info</code> là hàng đợi của Gazebo Transport đầy ở publisher đó;
Gazebo bỏ một message để queue không tăng vô hạn. Nó không chứng minh C API hay move_base2 bị lỗi. Tuy vậy,
nó là dấu hiệu nên theo dõi tải mô phỏng/consumer. Cảnh báo ảnh hưởng an toàn hơn trong log là:</p>
<pre>/camera_right/depth/points_proc observation buffer has not been updated ...
[move_base2] Sensor data is stale — wheel commands blocked</pre>
<p>move_base2 đang dừng lệnh bánh xe đúng chủ đích vì local/global costmap không còn mô tả thế giới hiện tại.
Cần chẩn đoán đường camera phải/DepthCameraData, callback SensorConverter và tần số thực tế; không quy kết ngay
cho nghẽn mạng.</p>
<h2>7. Dấu vết mã nguồn dùng để kết luận</h2>
<ul class="small">
<li><code>Controllers/Packages/amr_control/src/amr_control.cpp</code>: host tìm library <code>MoveBase</code> và import factory alias.</li>
<li><code>Test/move_base2/src/move_base2_plugin.cpp</code>: factory trả <code>BaseNavigation::Ptr</code>, export cả <code>MoveBase2</code> và <code>MoveBase</code>.</li>
<li><code>Test/move_base2/launch/move_base2_control.launch</code>: đặt <code>PNKX_NAV_CORE_CONFIG_DIR</code> sang overlay runtime.</li>
<li><code>Test/move_base2/config/runtime/move_base_common_params.yaml</code>: <code>MoveBase.library_path = libmove_base2</code>.</li>
<li><code>pnkx_nav_core/src/APIs/c_api/src/nav_c_api.cpp</code>: <code>navigation_create()</code> dùng chính alias <code>MoveBase</code>.</li>
<li><code>pnkx_nav_core/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h</code>: contract chung <code>BaseNavigation</code>.</li>
<li><code>Test/move_base2/src/navigation_runtime.cpp</code>: xây costmap, runners và mission layer.</li>
</ul>
</body>
</html>

BIN
docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf Normal file
View File

Binary file not shown.

57
docs/STATE_MACHINE.md
View File

@@ -11,9 +11,9 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
| State | Ai phát cmd_vel | Vào state khi | Ra khi |
|---|---|---|---|
| `IDLE` | không ai (0) | khởi động; cycle ngay sau một state terminal | có `NavigationRequest` đang chờ → `PLANNING`; yêu cầu chỉ-có-action (`has_goal == false`, D8) → `EXECUTING_ACTIONS`; yêu cầu không có goal lẫn action → `ABORTED` (tự vệ) |
| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; quá `planner_patience` hoặc quá `max_planning_retries` → `RECOVERING(planning_failed)` |
| `PLANNING` | không ai (0) | nhận yêu cầu; controller không sinh được lệnh mà chưa hết kiên nhẫn; recovery vừa chạy xong; tiếp tục sau tạm dừng | có plan hợp lệ → `CONTROLLING`; global planner chính fail/plan rỗng và có backup chưa dùng → đổi sang backup, lập plan lại; backup fail (hoặc không có backup) / quá `planner_patience` / quá `max_planning_retries` → `RECOVERING(planning_failed)` |
| `CONTROLLING` | **local planner** | có plan hợp lệ; tiếp tục sau tạm dừng | `isGoalReached` → `SUCCEEDED` (hết action) hoặc `EXECUTING_ACTIONS` (còn action — D8); quá `controller_patience` → `RECOVERING(controlling_failed)`; quá `oscillation_timeout` → `RECOVERING(oscillation)`; không sinh được lệnh (còn kiên nhẫn, còn pose) → `PLANNING` |
| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, chỉ số behavior tăng 1; hết behavior khi định vào → `ABORTED` |
| `RECOVERING` | **recovery behavior** | ba trigger ở trên | tick trả `succeeded`/`failed` → `PLANNING`, cursor của **route thuộc trigger đó** tăng 1; hết route khi định vào → `ABORTED` |
| `EXECUTING_ACTIONS` | không ai (0) — **D8** | tới goal mà yêu cầu còn action; nhận yêu cầu `has_goal == false` | action xong mà còn action kế → start action kế (ở nguyên state); action cuối `succeeded` → `SUCCEEDED`; một action `failed` → `ABORTED` (không qua recovery); quá `action_patience` (nếu bật) → cancel action + `ABORTED`; `cancel()` → `CANCELLING`; `pause()` → `PAUSED` (không huỷ action) |
| `PAUSED` | không ai (0) | `pause()` từ `PLANNING`/`CONTROLLING`/`RECOVERING`/`EXECUTING_ACTIONS` | `resume()` → về state trước đó; `cancel()` → `CANCELLING` |
| `CANCELLING` | không ai (0) | `cancel()` từ mọi state đang chạy | robot đã dừng → `CANCELLED` |
@@ -76,7 +76,7 @@ Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi m
| `planning_retries_` (đếm `max_planning_retries`) | như trên | — |
| `last_valid_control_` (đo `controller_patience`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; recovery chạy xong; controller sinh được lệnh hợp lệ | **có plan mới** |
| `last_oscillation_reset_` (đo `oscillation_timeout`) | nhận yêu cầu mới; tiếp tục sau tạm dừng; robot đi được quá `oscillation_distance` | **có plan mới** |
| `recovery_index_` | nhận yêu cầu mới | — |
| cursor các recovery route | nhận yêu cầu mới | route của trigger khác; cursor chỉ tăng sau lượt recovery của chính trigger đó |
| `action_started_at_` (đo `action_patience`, D8) | start một action (trần tính cho TỪNG action); tiếp tục sau tạm dừng (quãng dừng không tính vào trần) | — |
Hai ô "không đặt lại khi có plan mới" là điểm dễ sai nhất và đã từng sai trong lúc thi công: nếu làm
@@ -84,6 +84,57 @@ mới hai đồng hồ đó mỗi lần có plan, vòng lặp `CONTROLLING → P
hạn, và một controller hỏng vĩnh viễn sẽ không bao giờ chạm `controller_patience`. Test
`ControllerPatienceSurvivesReplanLoop` giữ tính chất này.
## Global planner dự phòng
`backup_global_planner` là một alias tùy chọn ở root config. Khi global planner active trả `false`
hoặc plan rỗng, `ControlLoop` đổi sang alias này **một lần duy nhất cho mỗi request**, giữ nguyên
local planner và state `PLANNING`. Lượt backup thành công đi bình thường vào `CONTROLLING`; lượt
backup fail mới được đưa vào `PlannerFeedback::kFailed`, nên state machine đi theo recovery hiện có.
Backup dùng overload `makePlan(start, goal, plan)` (không mang VDA5050 `Order`) để
`SBPLLatticePlanner` dùng được khi `CustomPlanner` của position fail. Đây là đường lùi hình học:
không được kỳ vọng giữ trajectory/edge metadata riêng của `CustomPlanner`. Backup không kích hoạt
khi planner bị treo — worker plugin không có cancel cưỡng bức; `planner_patience` vẫn là hàng rào
cho trường hợp đó.
## Recovery routes
`recovery/behaviors` là registry toàn bộ plugin có thể dùng; `recovery/routes` chọn **tên instance**
theo trigger, không phụ thuộc thứ tự nạp plugin:
```yaml
recovery:
behaviors:
- {name: wait, type: WaitRecovery}
- {name: clear, type: ClearCostmapRecovery}
- {name: detour_path, type: DetourPathRecovery}
- {name: rotate, type: RotateRecovery}
- {name: back_up, type: BackUpRecovery}
routes:
planning_failed: [wait, clear, rotate, back_up]
controlling_failed: [wait, clear, detour_path, rotate, back_up]
oscillation: [detour_path, rotate, back_up]
```
Khi dựng runtime, `RecoveryRunner` nạp registry trước rồi resolve tên route thành index thật; chỉ
sau đó `NavigationRuntime` mới gán `recovery_behavior_count` và các route này vào
`StateMachineConfig`. Route phải khai đủ cả ba trigger, không rỗng sau resolve, không lặp tên và
không có trigger lạ. Schema cũ không có `routes` vẫn tương thích: cả ba trigger dùng toàn bộ registry
theo thứ tự nạp.
`DetourPathRecovery` hiện chưa có plugin/library. Vì vậy entry `detour_path` có thể được commit trước:
registry báo plugin thiếu, `RecoveryRunner` cảnh báo và bỏ riêng tên đó khỏi route; không bao giờ
đưa index giả vào state machine. Với config hiện tại, trước khi plugin được thêm, route hữu hiệu là
`planning=[wait, clear, rotate, back_up]`, `controlling=[wait, clear, rotate, back_up]`,
`oscillation=[rotate, back_up]`. Khi plugin SBPL được nạp thành công, hai route sau tự có
`detour_path`, không cần sửa move_base2.
Mỗi request giữ ba cursor độc lập. Một lượt behavior `succeeded` **hoặc** `failed` luôn quay về
`PLANNING` để lập đường mới và tiêu thụ một phần tử của route đã kích hoạt; lần lỗi kế tiếp cùng
trigger thử phần tử sau. Lỗi bởi trigger khác dùng cursor của route khác. Log runner ghi `registry
index`, không phải vị trí trong route, để không đánh lừa vận hành khi một behavior bị dùng ở nhiều
route.
## Mất pose (TF thiếu hoặc quá hạn)
Không biết robot đang ở đâu thì không được cho nó chạy. Cụ thể:

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_
};

6
launch/move_base2_control.launch
View File

@@ -26,7 +26,8 @@
<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find move_base2)/config/runtime" />
<param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config" />
<rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
<!-- subst_value: mqtt_general.yaml dung $(find amr_startup) cho duong dan factsheet -->
<rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" subst_value="true" />
<node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
<rosparam param="footprint" if="$(eval robot_type == 'imr')">
@@ -45,7 +46,8 @@
<param name="primitive_filename" value="$(arg primitive_filename)" />
<param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
<rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
<!-- maker_sources is loaded by robot::NodeHandle from this launch's move_base2 runtime
overlay. Do not also load amr_startup's legacy copy: its contents diverge. -->
<rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />

4
package.xml
View File

@@ -82,4 +82,8 @@
<build_depend>mission_adapters</build_depend>
<run_depend>mission_adapters</run_depend>
<!-- ActionRunner nạp ActionHandler plugin qua action_core::ActionRegistry. -->
<build_depend>action_core</build_depend>
<run_depend>action_core</run_depend>
</package>

171
plugins/noop_action_handler.cpp
View File

@@ -1,171 +0,0 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — action handler mặc định: log rồi báo xong sau một khoảng thời gian.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/runners/action_handler.h>
#include <cmath>
#include <memory>
#include <string>
#include <vector>
#include <boost/dll/alias.hpp>
#include <robot/robot.h>
namespace move_base2
{
namespace
{
constexpr double kDefaultDuration = 0.0; // [s] 0 = xong ngay ở tick đầu.
constexpr double kMaxDuration = 600.0; // [s] trần vệ sinh cho param cấu hình sai.
constexpr double kDefaultTimeout = 30.0; // [s]
} // namespace
/**
* @class NoopActionHandler
* @brief Handler mặc định — không điều khiển thiết bị nào, chỉ log và đếm giờ.
*
* Có ba công dụng thật, không phải chỗ giữ chỗ:
* 1. cho `actionType` chưa có thiết bị tương ứng (`wait`, hoặc action chỉ mang ý nghĩa ghi nhật ký)
* chạy được mà không phải viết handler riêng;
* 2. dựng một deployment chạy end-to-end trước khi phần cứng sẵn sàng;
* 3. làm ví dụ tham chiếu cho contract — đặc biệt là **timeout tầng 1**.
*
* `duration = 0` nghĩa là xong ngay ở tick đầu. Đặt > 0 để mô phỏng một thiết bị chậm; đặt
* `hang: true` để mô phỏng thiết bị không bao giờ trả lời — khi đó chỉ `timeout` cắt được, đúng
* tình huống mà timeout tầng 1 sinh ra để xử lý.
*/
class NoopActionHandler final : public ActionHandler
{
public:
NoopActionHandler() = default;
static ActionHandler::Ptr create()
{
return std::make_shared<NoopActionHandler>();
}
bool configure(const std::string& name, robot::NodeHandle& nh) override
{
name_ = name;
nh.param("duration", duration_, kDefaultDuration);
nh.param("timeout", timeout_, kDefaultTimeout);
nh.param("hang", hang_, false);
nh.param("action_types", action_types_, std::vector<std::string>{"wait"});
if (!std::isfinite(duration_) || duration_ < 0.0 || duration_ > kMaxDuration)
{
robot::log_warning("[move_base2] '%s': duration=%.3f s ngoài [0, %.0f]; dùng %.3f s.",
name_.c_str(), duration_, kMaxDuration, kDefaultDuration);
duration_ = kDefaultDuration;
}
if (!std::isfinite(timeout_) || timeout_ < 0.0)
{
robot::log_warning("[move_base2] '%s': timeout=%.3f s không hợp lệ; dùng %.3f s.",
name_.c_str(), timeout_, kDefaultTimeout);
timeout_ = kDefaultTimeout;
}
if (hang_ && timeout_ <= 0.0)
{
// Chế độ mô phỏng thiết bị treo mà lại tắt timeout thì action sẽ chạy vĩnh viễn — đúng thứ
// contract cấm.
robot::log_error("[move_base2] '%s': hang=true nhưng timeout=%.3f s; handler sẽ không bao "
"giờ kết thúc.", name_.c_str(), timeout_);
return false;
}
if (!hang_ && timeout_ > 0.0 && duration_ > 0.0 && timeout_ <= duration_)
{
// Cấu hình này khiến action LUÔN hỏng vì timeout — gần như chắc chắn là gõ nhầm.
robot::log_error("[move_base2] '%s': timeout=%.3f s <= duration=%.3f s; action sẽ luôn thất "
"bại.", name_.c_str(), timeout_, duration_);
return false;
}
if (action_types_.empty())
{
robot::log_error("[move_base2] '%s': action_types rỗng — handler sẽ không bao giờ được gọi.",
name_.c_str());
return false;
}
return true;
}
std::vector<std::string> supportedActionTypes() const override
{
return action_types_;
}
bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) override
{
started_at_ = now;
action_id_ = action.actionId;
robot::log_info("[move_base2] '%s': bắt đầu action '%s' (id '%s'), duration %.3f s.",
name_.c_str(), action.actionType.c_str(), action.actionId.c_str(), duration_);
return true;
}
ActionTick update(const robot::Time& now) override
{
ActionTick tick;
const double elapsed = (now - started_at_).toSec(); // [s]
// Timeout TẦNG 1 — trách nhiệm của chính handler, không dựa vào action_patience của state
// machine (mặc định tắt). Ở đây ngưỡng là cấu hình vì handler này không nói chuyện với thiết bị
// nào; handler thật thì suy ngưỡng từ hiểu biết về thiết bị của nó.
if (timeout_ > 0.0 && elapsed >= timeout_)
{
tick.status = ActionTick::Status::kFailed;
tick.message = "action '" + action_id_ + "' quá timeout";
return tick;
}
if (hang_)
{
// Mô phỏng thiết bị không bao giờ trả lời. Có để fault-injection trong test tích hợp: đây là
// đúng tình huống mà timeout tầng 1 sinh ra để xử lý.
tick.status = ActionTick::Status::kRunning;
return tick;
}
if (elapsed >= duration_)
{
tick.status = ActionTick::Status::kSucceeded;
tick.message = "action '" + action_id_ + "' hoàn tất";
return tick;
}
tick.status = ActionTick::Status::kRunning;
return tick;
}
void cancel() override
{
// Không có thiết bị nào để đưa về trạng thái an toàn. Handler thật phải làm việc đó ở đây.
robot::log_info("[move_base2] '%s': action '%s' bị huỷ.", name_.c_str(), action_id_.c_str());
}
private:
std::string name_;
std::vector<std::string> action_types_{"wait"};
double duration_ = kDefaultDuration; ///< [s]
double timeout_ = kDefaultTimeout; ///< [s] 0 = không giới hạn
bool hang_ = false; ///< true = không bao giờ hoàn tất (fault injection)
robot::Time started_at_;
std::string action_id_;
};
} // namespace move_base2
BOOST_DLL_ALIAS(move_base2::NoopActionHandler::create, NoopActionHandler)

76
src/bridges/mission_adapter_bridge.cpp
View File

@@ -15,6 +15,33 @@
namespace move_base2
{
namespace
{
/// @brief Dịch gợi ý chuỗi của mission layer sang profile của navigation.
MotionProfile toProfile(const std::string& hint)
{
if (hint.empty() || hint == "position")
{
return MotionProfile::kPosition;
}
if (hint == "docking")
{
return MotionProfile::kDocking;
}
if (hint == "go_straight")
{
return MotionProfile::kGoStraight;
}
if (hint == "rotate")
{
return MotionProfile::kRotate;
}
robot::log_warning("[move_base2] MissionAdapterBridge: unknown motion_hint '%s' — running the leg "
"as a plain position goal.\n", hint.c_str());
return MotionProfile::kPosition;
}
} // namespace
MissionAdapterBridge::MissionAdapterBridge() = default;
MissionAdapterBridge::~MissionAdapterBridge() = default;
@@ -49,10 +76,6 @@ NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Missio
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());
@@ -61,10 +84,39 @@ NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Missio
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;
// Mission layer chở `motion_hint` dưới dạng CHUỖI và không diễn giải nó; đây là biên duy nhất
// dịch sang khái niệm của navigation. Mission nav mới luôn đặt profile hiệu lực; chuỗi rỗng vẫn
// được coi là position để tương thích nguồn mission cũ.
request.profile = toProfile(mission.motion_hint);
request.marker = mission.marker;
// Đích đến muộn đi qua nguyên vẹn; `ControlLoop::submit` mới là chỗ quy về pose tuyệt đối.
request.goal_frame = mission.goal_frame;
request.relative_distance = mission.relative_distance;
// Chặng của order PHẢI mang theo phần order của nó. Global planner cấu hình cho profile position
// là `CustomPlanner`, mà lớp này chỉ hiện thực nhánh `makePlan(Order, start, goal, plan)`; nhánh
// ba tham số là stub in "This function is not available!" rồi trả false
// (`custom_planner.h:86-91`). Chặng không có order vì thế fail ngay lượt lập plan đầu tiên và đi
// thẳng vào recovery cho tới `ABORTED` — không có dấu hiệu nào chỉ ra thiếu dữ liệu.
//
// Order dựng lại từ CHẶNG chứ không phải order gốc: planner tra edge theo `startNodeId`/
// `endNodeId` trong tập node nó nhận được, nên đưa cả order gốc xuống là mọi chặng đều lập plan
// cho toàn tuyến. Quỹ đạo NURBS của edge đi theo nguyên vẹn vì đây là bản sao của chính các edge
// trong order.
//
// CHỈ chặng position mới mang order. `DockPlanner` và `TwoPointsPlanner` — global planner của các
// profile còn lại — chỉ hiện thực nhánh `makePlan` ba tham số; mang order xuống thì `PlannerRunner`
// gọi biến thể `Order`, base trả false, và chặng fail ngay lượt lập plan đầu. Đúng lỗi đã xảy ra
// với `CustomPlanner` ngày 2026-07-31, soi gương lại.
if (mission.type == mission_adapters::MissionType::VDA5050_ORDER && mission.has_goal &&
request.profile == MotionProfile::kPosition)
{
auto order = std::make_shared<robot_protocol_msgs::Order>();
order->nodes = mission.nodes;
order->edges = mission.edges;
request.order = std::move(order);
}
return request;
}
@@ -87,8 +139,8 @@ bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters
{
// 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));
robot::log_warning("[move_base2] MissionAdapterBridge: rejecting mission %llu — the bridge did "
"not start.\n", static_cast<unsigned long long>(mission->id));
return false;
}
@@ -97,8 +149,8 @@ bool MissionAdapterBridge::dispatch(const std::shared_ptr<const mission_adapters
// 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),
robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu overwrites mission %llu "
"before it was pushed down.\n", static_cast<unsigned long long>(mission->id),
static_cast<unsigned long long>(pending_->id));
}

190
src/bridges/mission_layer.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt MissionLayer.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/bridges/mission_layer.h>
#include <mission_adapters/mission_request.h>
#include <robot/robot.h>
namespace move_base2
{
MissionLayer::MissionLayer() : events_(manager_, registry_), executor_(manager_)
{
}
MissionLayer::~MissionLayer()
{
stop();
}
bool MissionLayer::configure(robot::NodeHandle& nh, const std::string& ns, std::string& error)
{
if (active_)
{
error = "MissionLayer::configure() called twice";
return false;
}
if (ns.empty())
{
error = "mission_namespace is empty";
return false;
}
if (!config_.loadFromParams(nh, ns))
{
error = "invalid mission parameters in namespace '" + ns + "'";
return false;
}
manager_.setConfig(config_);
// Nạp hụt một nguồn không xoá các nguồn còn lại: registry đã log đích danh nguồn hỏng và lý do.
if (!registry_.loadFromConfig(nh, ns))
{
robot::log_warning("[move_base2] MissionLayer: some mission sources failed to load; "
"continuing with the remaining %zu source(s).\n", registry_.size());
}
if (registry_.size() == 0)
{
error = "no mission source could be loaded from namespace '" + ns +
"' — check `mission_sources` and the `library_path` key of each type";
return false;
}
active_ = true;
return true;
}
void MissionLayer::attach(MissionAdapterBridge& bridge)
{
// Hai chiều, cả hai đều non-owning: bridge báo outcome lên manager, executor đẩy chặng qua bridge.
bridge.attach(&manager_);
executor_.setNavigationClient(&bridge);
}
void MissionLayer::start()
{
if (!active_ || started_)
{
return;
}
// Event thread trước executor: nguồn sinh việc phải sẵn sàng trước nơi tiêu thụ việc. Ngược lại
// thì executor chạy một vòng rỗng rồi ngủ, vô hại nhưng không có lý do gì để làm vậy.
events_.start();
executor_.start();
started_ = true;
robot::log_info("[move_base2] MissionLayer started with %zu mission source(s).\n",
registry_.size());
}
void MissionLayer::stop()
{
if (!started_)
{
return;
}
// Ngược chiều dòng dữ liệu: chặn nguồn sự kiện trước, rồi mới dừng nơi phát lệnh xuống navigation.
// Dừng executor trước thì event thread vẫn nạp thêm mission vào hàng đợi của một hệ đang tắt.
events_.stop();
executor_.stop();
started_ = false;
}
bool MissionLayer::handles(const std::string& schema) const
{
return registry_.find(schema) != nullptr;
}
bool MissionLayer::submitOrder(const robot_protocol_msgs::Order& order)
{
if (!started_ || !handles(mission_adapters::schema::kVda5050Order))
{
return false;
}
events_.orderEvent(order);
return true;
}
bool MissionLayer::submitGoal(const robot_geometry_msgs::PoseStamped& goal)
{
if (!started_ || !handles(mission_adapters::schema::kPoseStamped))
{
return false;
}
events_.goalEvent(goal);
return true;
}
void MissionLayer::cancel()
{
if (started_)
{
events_.cancelEvent();
}
}
void MissionLayer::pause()
{
if (started_)
{
events_.pauseEvent();
}
}
void MissionLayer::resume()
{
if (started_)
{
events_.resumeEvent();
}
}
void MissionLayer::emergency()
{
if (started_)
{
events_.emergencyEvent();
}
}
void MissionLayer::clearEmergency()
{
if (started_)
{
events_.clearEmergencyEvent();
}
}
bool MissionLayer::hasMission() const
{
return manager_.hasMission();
}
mission_adapters::MissionState MissionLayer::state() const
{
return manager_.state();
}
std::size_t MissionLayer::sourceCount() const
{
return registry_.size();
}
void MissionLayer::markActiveForTesting()
{
active_ = registry_.size() > 0;
}
} // namespace move_base2

325
src/config/move_base2_config.cpp
View File

@@ -9,6 +9,7 @@
#include <sstream>
#include <robot/robot.h>
#include <yaml-cpp/yaml.h>
namespace move_base2
{
@@ -20,7 +21,7 @@ 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);
robot::log_warning("[move_base2] missing param '%s', using default %.4f", key.c_str(), value);
return;
}
nh.param(key, value, value);
@@ -30,7 +31,7 @@ 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);
robot::log_warning("[move_base2] missing param '%s', using default %d", key.c_str(), value);
return;
}
nh.param(key, value, value);
@@ -40,7 +41,7 @@ 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(),
robot::log_warning("[move_base2] missing param '%s', using default %s", key.c_str(),
value ? "true" : "false");
return;
}
@@ -51,7 +52,7 @@ void readString(robot::NodeHandle& nh, const std::string& key, std::string& valu
{
if (!nh.hasParam(key))
{
robot::log_warning("[move_base2] thiếu param '%s', dùng default '%s'", key.c_str(),
robot::log_warning("[move_base2] missing param '%s', using default '%s'", key.c_str(),
value.c_str());
return;
}
@@ -64,28 +65,121 @@ void readBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& b
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);
}
void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors);
/// Đọc cặp planner runtime trực tiếp, không có adapter gen-1 ở giữa.
void readRootProfileBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& binding)
{
robot::NodeHandle profile_nh(nh, ns);
readString(profile_nh, "global_planner", binding.global_planner_name);
readString(profile_nh, "local_planner", binding.local_planner_name);
}
void readDockingMarkerProfiles(robot::NodeHandle& nh, DockingMarkerProfiles& profiles,
std::string& error)
{
profiles.clear();
error.clear();
if (!nh.hasParam("docking_marker_profiles"))
{
return; // Optional: the default docking binding remains authoritative.
}
const YAML::Node table = nh.getParamValue("docking_marker_profiles");
if (!table || !table.IsDefined())
{
error = "docking_marker_profiles is declared but cannot be read";
return;
}
if (!table.IsMap())
{
error = "docking_marker_profiles must be a map of marker names";
return;
}
try
{
for (auto entry = table.begin(); entry != table.end(); ++entry)
{
const std::string marker = entry->first.as<std::string>();
const YAML::Node value = entry->second;
if (marker.empty() || !value.IsMap())
{
error = "docking_marker_profiles has an empty marker name or a non-map entry";
return;
}
if (!value["global_planner"] || !value["local_planner"] ||
!value["global_planner"].IsScalar() || !value["local_planner"].IsScalar())
{
error = "docking_marker_profiles/'" + marker +
"' must set scalar global_planner and local_planner";
return;
}
ProfileBinding binding;
binding.global_planner_name = value["global_planner"].as<std::string>();
binding.local_planner_name = value["local_planner"].as<std::string>();
if (binding.global_planner_name.empty() || binding.local_planner_name.empty())
{
error = "docking_marker_profiles/'" + marker +
"' has an empty global_planner or local_planner";
return;
}
profiles.emplace(marker, std::move(binding));
}
}
catch (const YAML::Exception& ex)
{
error = std::string("docking_marker_profiles is malformed: ") + ex.what();
}
}
/// Các tham số chung của move_base2, dùng chung cho schema namespace và schema root-profile.
void readMoveBase2Fields(robot::NodeHandle& nh, MoveBase2Config& config)
{
readDouble(nh, "controller_frequency", config.controller_frequency);
readDouble(nh, "planner_frequency", config.planner_frequency);
readDouble(nh, "planner_timeout", config.planner_timeout);
readDouble(nh, "runtime_stats_period", config.runtime_stats_period);
readDouble(nh, "planner_patience", config.state_machine.planner_patience);
readDouble(nh, "controller_patience", config.state_machine.controller_patience);
readDouble(nh, "oscillation_timeout", config.state_machine.oscillation_timeout);
readDouble(nh, "oscillation_distance", config.state_machine.oscillation_distance);
readDouble(nh, "action_patience", config.state_machine.action_patience);
readInt(nh, "max_planning_retries", config.state_machine.max_planning_retries);
readBool(nh, "recovery_behavior_enabled", config.state_machine.recovery_enabled);
readDouble(nh, "max_vel_x", config.velocity.max_vel_x);
readDouble(nh, "min_vel_x", config.velocity.min_vel_x);
readDouble(nh, "max_vel_theta", config.velocity.max_vel_theta);
readDouble(nh, "acc_lim_x", config.velocity.max_accel_x);
readDouble(nh, "acc_lim_theta", config.velocity.max_accel_theta);
readSensors(nh, config.sensors);
readBool(nh, "docking_requires_marker", config.docking_requires_marker);
readString(nh, "recovery_namespace", config.recovery_namespace);
readString(nh, "action_namespace", config.action_namespace);
readString(nh, "mission_namespace", config.mission_namespace);
readBool(nh, "mission_layer_enabled", config.mission_layer_enabled);
readString(nh, "backup_global_planner", config.backup_global_planner_name);
readString(nh, "global_frame", config.global_frame);
readString(nh, "robot_base_frame", config.robot_base_frame);
readBool(nh, "require_current_costmap", config.require_current_costmap);
}
bool validateBinding(const ProfileBinding& binding, const char* name, std::string& error)
{
(void)name;
(void)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.
// Không đặt là hợp lệ: deployment có thể không dùng profile đó.
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;
}
@@ -101,134 +195,89 @@ void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors)
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";
}
/// Dịch patience gen-1 sang gen-2. Gen-1: mốc + patience luôn ở quá khứ khi patience <= 0, tức là
/// "fail -> recovery NGAY". Gen-2: <= 0 nghĩa là TẮT đồng hồ — ngược nghĩa hoàn toàn. Giữ hành vi
/// cũ bằng cách dịch thành đúng một chu kỳ điều khiển (gen-1 cũng chỉ phản ứng theo cycle).
double legacyPatience(double value, double control_period_s, const char* key)
{
if (value > 0.0)
{
return value;
}
robot::log_warning(
"[move_base2] legacy %s = %.3f: gen-1 hiểu là 'fail -> recovery ngay', gen-2 hiểu là 'tắt "
"đồng hồ'. Dịch thành một chu kỳ điều khiển (%.4f s) để giữ hành vi cũ.",
key, value, control_period_s);
return control_period_s;
}
/// Đọc binding của một profile theo schema gen-1: tên local planner ở khoá `<profile>_planner_name`
/// tại root, global planner ở section con mang TÊN planner đó (thiếu thì dùng global mặc định).
void readLegacyBinding(robot::NodeHandle& nh, const std::string& name_key,
const std::string& default_global, double xy_tolerance,
double yaw_tolerance, ProfileBinding& binding)
{
binding.default_xy_tolerance = xy_tolerance;
binding.default_yaw_tolerance = yaw_tolerance;
binding.global_planner_name = default_global;
// Default để RỖNG chứ không lấy default gen-1 ("mkt_algorithm/..."): các plugin đó không tồn tại
// trong workspace, và profile không khai coi như không dùng — validate sẽ chặn nếu cả bốn rỗng.
std::string local_name;
nh.param(name_key, local_name, std::string(""));
if (local_name.empty())
{
robot::log_warning("[move_base2] legacy: thiếu '%s' — profile này bị tắt", name_key.c_str());
return;
}
binding.local_planner_name = local_name;
robot::NodeHandle planner_nh(nh, local_name);
if (planner_nh.hasParam("base_global_planner"))
{
planner_nh.param("base_global_planner", binding.global_planner_name,
binding.global_planner_name);
}
robot::log_info("[move_base2] legacy: %s='%s' -> local='%s' global='%s'", name_key.c_str(),
local_name.c_str(), binding.local_planner_name.c_str(),
binding.global_planner_name.c_str());
<< binding.local_planner_name << "'\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);
readMoveBase2Fields(nh, *this);
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);
readDockingMarkerProfiles(nh, docking_marker_profiles, docking_marker_profiles_error);
// 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.
}
void MoveBase2Config::fromRootProfileNodeHandle(robot::NodeHandle& nh)
{
readMoveBase2Fields(nh, *this);
readRootProfileBinding(nh, "position", position);
readRootProfileBinding(nh, "docking", docking);
readRootProfileBinding(nh, "go_straight", go_straight);
readRootProfileBinding(nh, "rotate", rotate);
readDockingMarkerProfiles(nh, docking_marker_profiles, docking_marker_profiles_error);
}
bool MoveBase2Config::validate(std::string& error) const
{
if (!docking_marker_profiles_error.empty())
{
error = docking_marker_profiles_error;
return false;
}
if (!std::isfinite(controller_frequency) || controller_frequency <= 0.0)
{
error = "controller_frequency phải > 0 [Hz]";
error = "controller_frequency must be > 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";
error = "controller_frequency > 200 Hz — unrealistic rate for a control loop that owns a "
"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)";
error = "planner_frequency must be >= 0 [Hz] (0 = plan only when needed)";
return false;
}
if (!std::isfinite(planner_timeout))
{
error = "planner_timeout không hữu hạn [s]";
error = "planner_timeout is not finite [s]";
return false;
}
if (!std::isfinite(runtime_stats_period) || runtime_stats_period < 0.0)
{
error = "runtime_stats_period must be >= 0 [s] (0 = telemetry off)";
return false;
}
if (recovery_namespace.empty())
{
error = "recovery_namespace rỗng";
error = "recovery_namespace is empty";
return false;
}
if (mission_layer_enabled && mission_namespace.empty())
{
error = "mission_layer_enabled is true but mission_namespace is empty — there would be no "
"namespace to read `mission_sources` from";
return false;
}
if (global_frame.empty() || robot_base_frame.empty())
{
error = "global_frame và robot_base_frame không được rỗng";
error = "global_frame and robot_base_frame must not be empty";
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ạ độ";
error = "global_frame equals robot_base_frame — the robot pose would always be the origin";
return false;
}
@@ -243,7 +292,7 @@ bool MoveBase2Config::validate(std::string& error) const
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";
error = "no profile has base_local_planner — the runtime would reject every request";
return false;
}
@@ -278,13 +327,26 @@ std::string MoveBase2Config::describe() const
out << " controller_frequency: " << controller_frequency << " Hz\n";
out << " planner_frequency: " << planner_frequency << " Hz\n";
out << " planner_timeout: " << planner_timeout << " s\n";
out << " runtime_stats_period: " << runtime_stats_period << " s"
<< (runtime_stats_period > 0.0 ? "\n" : " (off)\n");
out << " frames: global='" << global_frame << "' base='" << robot_base_frame << "'\n";
out << " require_current_costmap: " << (require_current_costmap ? "true" : "false") << "\n";
out << " namespaces: recovery='" << recovery_namespace << "' actions='" << action_namespace
<< "' mission='" << mission_namespace << "'\n";
out << " mission_layer_enabled: " << (mission_layer_enabled ? "true" : "false")
<< (mission_layer_enabled ? " (orders are split into legs)\n"
: " (orders go straight down as one goal)\n");
describeBinding(out, "position", position);
describeBinding(out, "docking", docking);
for (const auto& entry : docking_marker_profiles)
{
describeBinding(out, ("docking marker '" + entry.first + "'").c_str(), entry.second);
}
out << " docking_requires_marker: " << (docking_requires_marker ? "true" : "false") << '\n';
describeBinding(out, "go_straight", go_straight);
describeBinding(out, "rotate", rotate);
out << " backup_global_planner: '" << backup_global_planner_name << "'"
<< (backup_global_planner_name.empty() ? " (off)\n" : "\n");
out << state_machine.describe();
out << velocity.describe();
out << sensors.describe();
@@ -298,14 +360,14 @@ std::string MoveBase2Config::describe() const
void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
{
// Default của gen-1 khác gen-2 ở hai chỗ; chế độ legacy giữ default gen-1 để không đổi hành vi
// của một hệ đang chạy chỉ vì đổi runtime.
// Schema gen-1 dùng base_footprint; giữ frame cũ để không đổi hành vi của hệ đang chạy.
robot_base_frame = "base_footprint";
position.default_xy_tolerance = 0.2; // [m]
position.default_yaw_tolerance = 0.2; // [rad]
readDouble(nh, "controller_frequency", controller_frequency);
readDouble(nh, "planner_frequency", planner_frequency);
// Khoá này không có trong schema gen-1 — nó là công cụ chẩn đoán của move_base2. Vẫn đọc ở đây
// để bật được telemetry mà không phải chuyển cả cây config sang schema mới.
readDouble(nh, "runtime_stats_period", runtime_stats_period);
readDouble(nh, "planner_patience", state_machine.planner_patience);
readDouble(nh, "controller_patience", state_machine.controller_patience);
readDouble(nh, "oscillation_timeout", state_machine.oscillation_timeout);
@@ -315,12 +377,16 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
readString(nh, "global_frame", global_frame);
readString(nh, "robot_base_frame", robot_base_frame);
// Không có trong schema gen-1 (bản cũ hard-code guard này, không cho tắt) — đọc để có đường tắt
// khi observation buffer bị cấu hình sai, mặc định vẫn là bật.
readBool(nh, "require_current_costmap", require_current_costmap);
// Sai số ở root là default chung cho cả bốn profile.
double xy = position.default_xy_tolerance;
double yaw = position.default_yaw_tolerance;
readDouble(nh, "xy_goal_tolerance", xy);
readDouble(nh, "yaw_goal_tolerance", yaw);
// Cũng không có trong schema gen-1: gen-1 không có mission layer nên không có khoá tương ứng để
// dịch. Đọc ở đây để tắt/bật được ngay trên cây config đang chạy mà không phải chuyển schema —
// đây là đường lùi khi mission layer gây vấn đề trên hiện trường.
readBool(nh, "mission_layer_enabled", mission_layer_enabled);
readString(nh, "mission_namespace", mission_namespace);
readBool(nh, "docking_requires_marker", docking_requires_marker);
std::string root_global_planner;
readString(nh, "base_global_planner", root_global_planner);
@@ -332,8 +398,9 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
// `LocalPlannerAdapter` 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 nên không cần cầu đó — bỏ qua CÓ LOG, để không ai tưởng
// khoá này vẫn đang có hiệu lực.
robot::log_warning("[move_base2] schema gen-1: bỏ qua base_local_planner='%s' — move_base2 gọi "
"thẳng local planner, không qua adapter.", adapter.c_str());
robot::log_warning("[move_base2] schema gen-1: ignoring base_local_planner='%s' — move_base2 "
"calls the local planner directly, not through an adapter.",
adapter.c_str());
}
struct LegacyProfile
@@ -350,12 +417,10 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
for (const LegacyProfile& profile : profiles)
{
profile.binding->default_xy_tolerance = xy;
profile.binding->default_yaw_tolerance = yaw;
if (!nh.hasParam(profile.key))
{
robot::log_warning("[move_base2] schema gen-1: thiếu '%s', profile này sẽ từ chối mọi yêu cầu",
robot::log_warning("[move_base2] schema gen-1: missing '%s', this profile will reject every "
"request",
profile.key);
continue;
}
@@ -385,14 +450,14 @@ void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh)
const double one_cycle = controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05; // [s]
if (state_machine.planner_patience <= 0.0)
{
robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 được dịch thành %.3f s "
"(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 is translated into %.3f s "
"(one control cycle), not into 'off'.", one_cycle);
state_machine.planner_patience = one_cycle;
}
if (state_machine.controller_patience <= 0.0)
{
robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 được dịch thành %.3f s "
"(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle);
robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 is translated into "
"%.3f s (one control cycle), not into 'off'.", one_cycle);
state_machine.controller_patience = one_cycle;
}
}
@@ -407,21 +472,29 @@ MoveBase2Config MoveBase2Config::load(robot::NodeHandle& root_nh)
robot::NodeHandle modern_nh(root_nh, "move_base2");
if (modern_nh.hasParam("controller_frequency"))
{
robot::log_info("[move_base2] dùng schema mới (namespace 'move_base2').");
robot::log_info("[move_base2] using the new schema (namespace 'move_base2').");
config.fromNodeHandle(modern_nh);
return config;
}
robot::NodeHandle position_nh(root_nh, "position");
if (position_nh.hasParam("local_planner"))
{
robot::log_info("[move_base2] using the root profile schema.");
config.fromRootProfileNodeHandle(root_nh);
return config;
}
if (root_nh.hasParam("controller_frequency") || root_nh.hasParam("base_global_planner"))
{
robot::log_warning("[move_base2] không thấy namespace 'move_base2'; đọc theo schema gen-1 của "
robot::log_warning("[move_base2] namespace 'move_base2' not found; reading the gen-1 schema of "
"move_base_common_params.yaml.");
config.fromLegacyNodeHandle(root_nh);
return config;
}
robot::log_error("[move_base2] không tìm thấy cấu hình nào — chạy với toàn bộ giá trị mặc định. "
"Kiểm PNKX_NAV_CORE_CONFIG_DIR và sự tồn tại của file config.");
robot::log_error("[move_base2] no configuration found — running with all default values. Check "
"PNKX_NAV_CORE_CONFIG_DIR and that the config file exists.");
return config;
}
@@ -433,10 +506,14 @@ ControlLoopConfig MoveBase2Config::toControlLoopConfig() const
config.nominal_control_period =
controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05; // [s]
config.robot_base_frame = robot_base_frame;
config.require_current_costmap = require_current_costmap;
config.position = position;
config.docking = docking;
config.docking_marker_profiles = docking_marker_profiles;
config.go_straight = go_straight;
config.rotate = rotate;
config.backup_global_planner_name = backup_global_planner_name;
config.docking_requires_marker = docking_requires_marker;
return config;
}

204
src/control_loop.cpp
View File

@@ -21,6 +21,10 @@ 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;
/// [s] Giãn cách log cho guard "không đi mù". Tình trạng này kéo dài hàng giây, và đây là vòng lặp
/// điều khiển — log mỗi cycle sẽ nhấn chìm mọi dòng khác.
constexpr double kStaleCostmapLogThrottle = 5.0;
} // namespace
// ================================================================================================
@@ -39,19 +43,28 @@ bool ControlLoopConfig::validate(std::string& error) const
}
if (!(nominal_control_period > 0.0))
{
error = "nominal_control_period phải > 0 [s]";
error = "nominal_control_period must be > 0 [s]";
return false;
}
if (position.local_planner_name.empty())
{
error = "profile 'position' bắt buộc phải có local_planner_name";
error = "profile 'position' must have local_planner_name";
return false;
}
for (const auto& entry : docking_marker_profiles)
{
if (entry.first.empty() || entry.second.global_planner_name.empty() ||
entry.second.local_planner_name.empty())
{
error = "every docking marker profile must have a name, global_planner, and local_planner";
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";
error = "robot_base_frame must not be empty";
return false;
}
return true;
@@ -88,7 +101,7 @@ bool ControlLoop::configure(const ControlLoopConfig& config, const ControlLoopDe
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)";
error = "missing a required port (clock/pose/planner/controller/recovery)";
return false;
}
if (!config.validate(error))
@@ -129,6 +142,7 @@ void ControlLoop::reset()
latest_plan_.clear();
planner_running_ = false;
backup_global_planner_active_ = 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_;
@@ -145,14 +159,21 @@ void ControlLoop::reset()
last_reason_ = "";
}
const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile) const
const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile, const std::string& marker) const
{
switch (profile)
{
case MotionProfile::kPosition:
return &config_.position;
case MotionProfile::kDocking:
{
const auto it = config_.docking_marker_profiles.find(marker);
if (!marker.empty() && it != config_.docking_marker_profiles.end())
{
return &it->second;
}
return &config_.docking;
}
case MotionProfile::kGoStraight:
return &config_.go_straight;
case MotionProfile::kRotate:
@@ -172,18 +193,71 @@ bool ControlLoop::isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose
return std::abs(std::sqrt(norm_sq) - 1.0) <= kQuaternionNormTolerance;
}
bool ControlLoop::submit(const NavigationRequest& request, std::string& reason)
bool ControlLoop::resolveDeferredGoal(NavigationRequest& request, std::string& reason) const
{
const bool has_frame = !request.goal_frame.empty();
const bool has_distance = std::isfinite(request.relative_distance);
if (!has_frame && !has_distance)
{
return true; // Goal tuyệt đối, không có gì phải quy.
}
if (has_frame && has_distance)
{
// Hai nguồn đích cùng lúc thì không có thứ tự nào là hiển nhiên đúng. Từ chối thay vì chọn bừa.
reason = "request sets both goal_frame and relative_distance";
return false;
}
if (deps_.pose == nullptr)
{
reason = "deferred goal needs a pose port";
return false;
}
if (has_frame)
{
if (!deps_.pose->lookupPose(request.goal_frame, request.goal))
{
reason = "cannot resolve goal_frame '" + request.goal_frame + "'";
return false;
}
return true;
}
robot_geometry_msgs::PoseStamped robot_pose;
if (!deps_.pose->getRobotPose(robot_pose))
{
// Mất định vị thì không quy được quãng đường tương đối. Đoán ở đây là robot đi mù một đoạn.
reason = "relative_distance needs the robot pose, which is not available";
return false;
}
const auto& q = robot_pose.pose.orientation;
const double yaw = std::atan2(2.0 * (q.w * q.z + q.x * q.y),
1.0 - 2.0 * (q.y * q.y + q.z * q.z)); // [rad]
request.goal = robot_pose;
request.goal.pose.position.x += request.relative_distance * std::cos(yaw);
request.goal.pose.position.y += request.relative_distance * std::sin(yaw);
return true;
}
bool ControlLoop::submit(const NavigationRequest& incoming, std::string& reason)
{
// Bản sao: quy đổi goal đến muộn ghi vào chính request, mà bên gọi truyền const ref.
NavigationRequest request = incoming;
if (!initialized_)
{
reason = "runtime chưa khởi tạo";
reason = "runtime not initialized";
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";
reason = "request carries actions but the runtime has no action port";
return false;
}
@@ -192,53 +266,83 @@ bool ControlLoop::submit(const NavigationRequest& request, std::string& reason)
// 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";
reason = "request has neither goal nor action";
return false;
}
pending_request_ = request;
has_pending_request_ = true;
backup_global_planner_active_ = false;
cancel_requested_ = false;
return true;
}
// Đích đến muộn: quy về pose tuyệt đối NGAY TẠI ĐÂY, trước mọi phép kiểm bên dưới. Đây là nơi duy
// nhất vừa biết chặng vừa được kích hoạt, vừa có cổng pose — mission layer sinh chặng lúc robot
// còn chưa tới nơi nên không thể quy sớm hơn.
if (!resolveDeferredGoal(request, reason))
{
return false;
}
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";
reason = "goal has non-finite coordinates";
return false;
}
if (!isQuaternionValid(request.goal))
{
reason = "goal có quaternion không hợp lệ";
reason = "goal has an invalid quaternion";
return false;
}
const ProfileBinding* binding = bindingFor(request.profile);
const ProfileBinding* binding = bindingFor(request.profile, request.marker);
if (binding == nullptr || binding->local_planner_name.empty())
{
reason = std::string("chưa cấu hình planner cho profile '") + toString(request.profile) + "'";
reason = std::string("no planner configured for profile '") + toString(request.profile) + "'";
return false;
}
// Marker phải được chọn TRƯỚC khi swap sang docking planner: planner legacy đọc `maker_name`
// trong initialize() (một lần), nên thứ tự ngược lại là dock vào marker của chặng trước.
// Compound action dùng goal_frame đã quy về pose tuyệt đối và HybridLocalPlanner không đọc
// maker_name, vì thế profile đó tắt requirement qua config thay vì bịa một marker.
if (request.profile == MotionProfile::kDocking)
{
if (request.marker.empty() && config_.docking_requires_marker)
{
reason = "docking request has no marker";
return false;
}
if (!request.marker.empty() && !deps_.controller->setDockingMarker(request.marker))
{
reason = "marker '" + request.marker + "' is invalid (not listed in maker_sources?)";
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 + "'";
reason = "could not load 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 + "'";
reason = "could not load 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);
robot::log_info("[move_base2] Mission %llu: profile=%s, marker='%s', global='%s', local='%s'.\n",
static_cast<unsigned long long>(request.mission_sequence_id),
toString(request.profile), request.marker.empty() ? "<default>" : request.marker.c_str(),
binding->global_planner_name.c_str(),
binding->local_planner_name.c_str());
pending_request_ = request;
has_pending_request_ = true;
backup_global_planner_active_ = false;
// 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;
@@ -290,6 +394,29 @@ void ControlLoop::collectPlannerResult()
// 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())
{
// Fallback thuộc policy của control loop, không phải recovery: planner chính đã kết thúc nên
// worker rảnh để nạp/chạy SBPL ngay cycle này. Chỉ thử một lần cho cả request; backup fail thì
// giữ kFailed để state machine đi đúng đường recovery hiện có.
if (!backup_global_planner_active_ && !config_.backup_global_planner_name.empty())
{
const std::string failed_planner = deps_.planner->activePlanner();
if (deps_.planner->swapPlanner(config_.backup_global_planner_name))
{
backup_global_planner_active_ = true;
planner_feedback_ = PlannerFeedback::kIdle;
robot::log_warning("[move_base2] global planner '%s' failed for mission %llu; switching "
"once to backup '%s'.\n",
failed_planner.c_str(),
static_cast<unsigned long long>(active_request_.mission_sequence_id),
config_.backup_global_planner_name.c_str());
return;
}
// Không có backup chạy được thì failure gốc vẫn phải đi recovery, không được để robot chờ.
robot::log_error("[move_base2] global planner '%s' failed and backup '%s' could not be "
"loaded; starting recovery.\n",
failed_planner.c_str(), config_.backup_global_planner_name.c_str());
}
planner_feedback_ = PlannerFeedback::kFailed;
return;
}
@@ -420,14 +547,14 @@ bool ControlLoop::step()
// Log một lần tại sườn nhận goal — không nằm trên đường lặp của control loop.
if (active_request_.has_goal)
{
robot::log_info("[move_base2] Nhận goal (mission %llu): x=%.3f y=%.3f frame=%s.\n",
robot::log_info("[move_base2] Goal received (mission %llu): x=%.3f y=%.3f frame=%s.\n",
static_cast<unsigned long long>(active_request_.mission_sequence_id),
active_request_.goal.pose.position.x, active_request_.goal.pose.position.y,
active_request_.goal.header.frame_id.c_str());
}
else
{
robot::log_info("[move_base2] Nhận yêu cầu chỉ-action (mission %llu), %zu action.\n",
robot::log_info("[move_base2] Action-only request received (mission %llu), %zu action(s).\n",
static_cast<unsigned long long>(active_request_.mission_sequence_id),
active_request_.actions.size());
}
@@ -540,7 +667,27 @@ bool ControlLoop::step()
}
}
if (output.run_controller)
// --- 3.5 Guard "không đi mù" -----------------------------------------------------------------
//
// Dữ liệu quan sát hết hạn nghĩa là costmap đang mô tả một thế giới của quá khứ. move_base thế hệ
// 1 chặn nguyên cycle tại đây (`move_base.cpp:2720`) và phát vận tốc 0; giữ nguyên ngữ nghĩa đó.
//
// Chỉ chặn hai thứ THỰC SỰ làm robot chạy: lời gọi controller và quyền phát vận tốc. Recovery vẫn
// được tick, có chủ đích — `ClearCostmapRecovery` chính là đường thoát đúng khi costmap hỏng, chặn
// nó đi là bịt mất lối phục hồi duy nhất còn tác dụng. Behavior họ velocity không bị thiệt vì
// chúng đo tiến độ bằng POSE: robot không nhúc nhích thì chúng tự hết giờ và báo hỏng, chứ không
// báo thành công nhầm.
const bool costmap_stale = config_.require_current_costmap && deps_.costmap_status != nullptr &&
!deps_.costmap_status->isCurrent();
if (costmap_stale)
{
// Throttle: đây là vòng lặp điều khiển, và tình trạng này kéo dài hàng giây.
robot::log_warning_throttle(kStaleCostmapLogThrottle,
"[move_base2] Sensor data is stale — wheel commands blocked (the "
"costmap is describing a past world).\n");
}
if (output.run_controller && !costmap_stale)
{
runController(candidate);
}
@@ -550,8 +697,11 @@ bool ControlLoop::step()
// `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_);
// SBPLLatticePlanner và nhiều planner tổng quát chỉ hiện thực overload ba tham số. Backup vì
// thế chủ đích không nhận Order; planner chính vẫn nhận Order đầy đủ (CustomPlanner).
const robot_protocol_msgs::Order* order =
backup_global_planner_active_ ? nullptr : active_request_.order.get();
planner_running_ = deps_.planner->startPlan(robot_pose, active_request_.goal, order, 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ồ
@@ -561,7 +711,7 @@ bool ControlLoop::step()
}
// --- 4. Lệnh vận tốc ------------------------------------------------------------------------
arbiter_.arbitrate(output.velocity_source, candidate, dt);
arbiter_.arbitrate(costmap_stale ? VelocitySource::kNone : output.velocity_source, candidate, dt);
// --- 5. Báo kết quả -------------------------------------------------------------------------
if (output.report_outcome)
@@ -579,15 +729,15 @@ bool ControlLoop::step()
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kPreempted:
robot::log_info("[move_base2] Goal bị thay bởi goal mới (mission %llu: PREEMPTED).\n",
robot::log_info("[move_base2] Goal replaced by a new goal (mission %llu: PREEMPTED).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kCancelled:
robot::log_info("[move_base2] Goal bị huỷ (mission %llu: CANCELLED).\n",
robot::log_info("[move_base2] Goal cancelled (mission %llu: CANCELLED).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kFailed:
robot::log_error("[move_base2] Navigation thất bại (mission %llu: ABORTED): %s\n",
robot::log_error("[move_base2] Navigation failed (mission %llu: ABORTED): %s\n",
static_cast<unsigned long long>(outgoing_mission_id),
last_reason_ != nullptr ? last_reason_ : "");
break;

9
src/io/costmap_exporter.cpp
View File

@@ -107,12 +107,21 @@ void CostmapExporter::prepareGridLocked()
}
}
void CostmapExporter::attachTelemetry(RuntimeStats* telemetry)
{
std::lock_guard<std::mutex> lock(mutex_);
telemetry_ = telemetry;
section_fill_ = (telemetry_ != nullptr) ? telemetry_->section("costmap.export")
: RuntimeStats::kInvalidSection;
}
void CostmapExporter::fill(robot_nav_msgs::OccupancyGrid& grid,
robot_map_msgs::OccupancyGridUpdate& /*update*/, bool& is_updated)
{
is_updated = false;
std::lock_guard<std::mutex> lock(mutex_);
ScopedSection timer(telemetry_, section_fill_);
if (costmap_ == nullptr)
{
return;

425
src/io/runtime_stats.cpp Normal file
View File

@@ -0,0 +1,425 @@
/**
* @file runtime_stats.cpp
* @brief Hiện thực @ref move_base2::RuntimeStats.
*/
#include <move_base2/io/runtime_stats.h>
#include <algorithm>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <sstream>
#ifdef __linux__
#include <dirent.h>
#include <sys/syscall.h>
#include <unistd.h>
#endif
#include <robot/robot.h>
namespace move_base2
{
namespace
{
/// Bề rộng cột nhãn của bảng — đủ cho `costmap/global_costmap` mà không xuống dòng.
constexpr int kLabelWidth = 26;
/// Tên hiển thị cho phần CPU không thuộc thread nào đã đăng ký (host ROS, ROS internals, plugin).
constexpr const char* kUnregisteredLabel = "(unregistered)";
/**
* @brief Đệm khoảng trắng bên phải cho đủ @p width **ký tự hiển thị**.
*
* `printf("%-*s")` đếm BYTE, mà nhãn ở đây có dấu tiếng Việt (UTF-8, 2 byte/ký tự) — dùng thẳng
* printf thì bảng lệch cột đúng bằng số dấu. Byte nối tiếp của UTF-8 luôn có dạng 10xxxxxx nên đếm
* byte KHÔNG phải continuation là ra số ký tự.
*/
std::string padRight(const std::string& text, int width)
{
int visible = 0;
for (const char ch : text)
{
if ((static_cast<unsigned char>(ch) & 0xC0) != 0x80)
{
++visible;
}
}
std::string padded = text;
for (int i = visible; i < width; ++i)
{
padded += ' ';
}
return padded;
}
double ticksPerSecond()
{
#ifdef __linux__
const long hz = sysconf(_SC_CLK_TCK);
return hz > 0 ? static_cast<double>(hz) : 100.0;
#else
return 100.0;
#endif
}
/**
* @brief Lấy utime+stime từ một dòng `/proc/.../stat`.
*
* Không tách theo khoảng trắng từ đầu dòng được: trường thứ hai là tên tiến trình, nằm trong ngoặc
* đơn và **có thể chứa cả khoảng trắng lẫn ngoặc**. Mốc đáng tin duy nhất là dấu `)` cuối cùng.
*/
std::uint64_t parseCpuTicks(const std::string& stat_line)
{
const std::size_t close = stat_line.rfind(')');
if (close == std::string::npos)
{
return 0;
}
std::istringstream iss(stat_line.substr(close + 1));
std::string field;
// Sau dấu ')' , trường đầu tiên là state; utime là trường thứ 12, stime thứ 13.
std::uint64_t utime = 0;
std::uint64_t stime = 0;
for (int index = 1; index <= 13; ++index)
{
if (!(iss >> field))
{
return 0;
}
if (index == 12)
{
utime = std::strtoull(field.c_str(), nullptr, 10);
}
else if (index == 13)
{
stime = std::strtoull(field.c_str(), nullptr, 10);
}
}
return utime + stime;
}
std::uint64_t readCpuTicksFrom(const std::string& path)
{
std::ifstream file(path);
if (!file.is_open())
{
return 0;
}
std::string line;
std::getline(file, line);
return parseCpuTicks(line);
}
} // namespace
RuntimeStats::RuntimeStats(double period_seconds)
: period_seconds_(period_seconds)
, ticks_per_second_(ticksPerSecond())
, window_start_(std::chrono::steady_clock::now())
{
if (!enabled())
{
return;
}
last_process_cpu_ticks_ = readProcessCpuTicks();
last_rss_bytes_ = readProcessRssBytes();
}
// ================================================================================================
// Đọc /proc
// ================================================================================================
std::uint64_t RuntimeStats::readThreadCpuTicks(long tid)
{
#ifdef __linux__
return readCpuTicksFrom("/proc/self/task/" + std::to_string(tid) + "/stat");
#else
(void)tid;
return 0;
#endif
}
std::uint64_t RuntimeStats::readProcessCpuTicks()
{
#ifdef __linux__
return readCpuTicksFrom("/proc/self/stat");
#else
return 0;
#endif
}
std::uint64_t RuntimeStats::readProcessRssBytes()
{
#ifdef __linux__
std::ifstream file("/proc/self/statm");
if (!file.is_open())
{
return 0;
}
std::uint64_t total_pages = 0;
std::uint64_t resident_pages = 0;
file >> total_pages >> resident_pages;
const long page_size = sysconf(_SC_PAGESIZE);
return resident_pages * static_cast<std::uint64_t>(page_size > 0 ? page_size : 4096);
#else
return 0;
#endif
}
std::vector<long> RuntimeStats::listThreadIds()
{
std::vector<long> tids;
#ifdef __linux__
DIR* dir = opendir("/proc/self/task");
if (dir == nullptr)
{
return tids;
}
while (const dirent* entry = readdir(dir))
{
if (entry->d_name[0] == '.')
{
continue;
}
tids.push_back(std::strtol(entry->d_name, nullptr, 10));
}
closedir(dir);
std::sort(tids.begin(), tids.end());
#endif
return tids;
}
// ================================================================================================
// Đăng ký
// ================================================================================================
RuntimeStats::SectionId RuntimeStats::section(const std::string& name)
{
if (!enabled())
{
return kInvalidSection;
}
std::lock_guard<std::mutex> lock(mutex_);
for (SectionId id = 0; id < sections_.size(); ++id)
{
if (sections_[id].name == name)
{
return id;
}
}
sections_.push_back(Section{ name, 0, 0, 0 });
return sections_.size() - 1;
}
void RuntimeStats::record(SectionId id, std::int64_t nanoseconds)
{
if (!enabled() || id == kInvalidSection)
{
return;
}
std::lock_guard<std::mutex> lock(mutex_);
if (id >= sections_.size())
{
return;
}
Section& section = sections_[id];
++section.calls;
section.total_ns += nanoseconds;
section.max_ns = std::max(section.max_ns, nanoseconds);
}
void RuntimeStats::registerCurrentThread(const std::string& label)
{
if (!enabled())
{
return;
}
#ifdef __linux__
// syscall trực tiếp thay cho gettid(): wrapper của glibc chỉ có từ 2.30, gọi thẳng thì không phụ
// thuộc phiên bản libc của máy build.
const long tid = static_cast<long>(syscall(SYS_gettid));
#else
const long tid = 0;
#endif
std::lock_guard<std::mutex> lock(mutex_);
for (Thread& thread : threads_)
{
if (thread.tid == tid)
{
thread.label = label;
return;
}
}
threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
}
void RuntimeStats::beginThreadCapture()
{
if (!enabled())
{
return;
}
std::lock_guard<std::mutex> lock(mutex_);
capture_before_ = listThreadIds();
capturing_ = true;
}
void RuntimeStats::endThreadCapture(const std::string& label)
{
if (!enabled())
{
return;
}
std::lock_guard<std::mutex> lock(mutex_);
if (!capturing_)
{
robot::log_warning("[move_base2] RuntimeStats::endThreadCapture('%s') without an open capture "
"window.\n",
label.c_str());
return;
}
capturing_ = false;
const std::vector<long> after = listThreadIds();
int labelled = 0;
for (const long tid : after)
{
if (std::binary_search(capture_before_.begin(), capture_before_.end(), tid))
{
continue;
}
threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
++labelled;
}
if (labelled == 0)
{
// Không phải lỗi chết người, nhưng phải nói ra: im lặng ở đây nghĩa là bảng thiếu hẳn một
// thành phần và người đọc lại tưởng thành phần đó không tốn gì.
robot::log_warning("[move_base2] RuntimeStats: '%s' created no thread — its CPU column will "
"not appear.\n",
label.c_str());
}
}
// ================================================================================================
// In bảng
// ================================================================================================
bool RuntimeStats::tick()
{
if (!enabled())
{
return false;
}
const auto now = std::chrono::steady_clock::now();
const double elapsed = std::chrono::duration<double>(now - window_start_).count();
if (elapsed < period_seconds_)
{
return false;
}
const std::string table = render();
robot::log_info("%s", table.c_str());
return true;
}
std::string RuntimeStats::render()
{
std::lock_guard<std::mutex> lock(mutex_);
const auto now = std::chrono::steady_clock::now();
const double window = std::chrono::duration<double>(now - window_start_).count();
const double safe_window = window > 1e-6 ? window : 1e-6;
const std::uint64_t process_ticks = readProcessCpuTicks();
const std::uint64_t rss_bytes = readProcessRssBytes();
const double process_cpu =
100.0 * static_cast<double>(process_ticks - last_process_cpu_ticks_) / ticks_per_second_ / safe_window;
const double rss_mb = static_cast<double>(rss_bytes) / (1024.0 * 1024.0);
const double rss_delta_mb = (static_cast<double>(rss_bytes) - static_cast<double>(last_rss_bytes_)) / (1024.0 * 1024.0);
const std::vector<long> all_tids = listThreadIds();
char line[256];
std::ostringstream out;
out << "\n";
std::snprintf(line, sizeof(line), "[move_base2] ===== runtime stats — window %.2f s =====\n",
window);
out << line;
std::snprintf(line, sizeof(line),
" process: RSS %.1f MB (%+.1f MB in window, %+.1f MB/min) CPU %.1f%% thread %zu\n",
rss_mb, rss_delta_mb, rss_delta_mb * 60.0 / safe_window, process_cpu, all_tids.size());
out << line;
// --- CPU theo thread ---------------------------------------------------------------------------
out << " " << padRight("thread", kLabelWidth) << " CPU%\n";
double registered_cpu = 0.0;
std::size_t registered_alive = 0;
for (Thread& thread : threads_)
{
const bool alive = std::binary_search(all_tids.begin(), all_tids.end(), thread.tid);
const std::uint64_t ticks = alive ? readThreadCpuTicks(thread.tid) : thread.last_cpu_ticks;
const double cpu =
100.0 * static_cast<double>(ticks - thread.last_cpu_ticks) / ticks_per_second_ / safe_window;
thread.last_cpu_ticks = ticks;
if (alive)
{
registered_cpu += cpu;
++registered_alive;
}
std::snprintf(line, sizeof(line), " %8.1f%s\n", cpu, alive ? "" : " (finished)");
out << " " << padRight(thread.label, kLabelWidth - 2) << line;
}
// Phần còn lại của tiến trình. Đây là con số quan trọng nhất khi đi tìm thủ phạm CPU: nếu nó lớn
// hơn hẳn tổng các thread đã đăng ký thì vấn đề KHÔNG nằm trong navigation stack.
const double other_cpu = process_cpu - registered_cpu;
std::snprintf(line, sizeof(line), " %8.1f (%zu thread)\n", other_cpu,
all_tids.size() > registered_alive ? all_tids.size() - registered_alive : 0);
out << " " << padRight(kUnregisteredLabel, kLabelWidth - 2) << line;
// --- Chi phí theo đoạn công việc ----------------------------------------------------------------
std::snprintf(line, sizeof(line), " %8s %10s %10s %8s\n", "calls/s", "avg [ms]", "peak [ms]",
"CPU%");
out << " " << padRight("work section", kLabelWidth) << line;
for (Section& section : sections_)
{
const double calls_per_second = static_cast<double>(section.calls) / safe_window;
const double avg_ms =
section.calls > 0 ? static_cast<double>(section.total_ns) / static_cast<double>(section.calls) / 1e6 : 0.0;
const double max_ms = static_cast<double>(section.max_ns) / 1e6;
// Tỷ lệ chiếm dụng: tổng thời gian đoạn này chạy so với chiều dài cửa sổ, quy ra %/1 core —
// cùng đơn vị với cột CPU% ở trên nên so sánh trực tiếp được.
const double share = 100.0 * static_cast<double>(section.total_ns) / 1e9 / safe_window;
std::snprintf(line, sizeof(line), " %8.1f %10.2f %10.2f %8.1f\n", calls_per_second, avg_ms, max_ms,
share);
out << " " << padRight(section.name, kLabelWidth - 2) << line;
section.calls = 0;
section.total_ns = 0;
section.max_ns = 0;
}
window_start_ = now;
last_process_cpu_ticks_ = process_ticks;
last_rss_bytes_ = rss_bytes;
return out.str();
}
} // namespace move_base2

43
src/io/sensor_gateway.cpp
View File

@@ -66,12 +66,12 @@ bool SensorGatewayConfig::validate(std::string& error) const
if (laser_sor_mean_k < 2)
{
error = "laser_sor_mean_k phải >= 2 [điểm] khi laser_sor_enabled = true";
error = "laser_sor_mean_k must be >= 2 [points] when 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";
error = "laser_sor_stddev_mul must be > 0 when laser_sor_enabled = true";
return false;
}
return true;
@@ -84,7 +84,7 @@ std::string SensorGatewayConfig::describe() const
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_mean_k : " << laser_sor_mean_k << " points\n";
out << " laser_sor_stddev_mul : " << laser_sor_stddev_mul << '\n';
}
return out.str();
@@ -125,6 +125,19 @@ bool SensorGateway::configure(const SensorGatewayConfig& config, std::string& er
return true;
}
void SensorGateway::attachTelemetry(RuntimeStats* telemetry)
{
telemetry_ = telemetry;
if (telemetry_ == nullptr)
{
return;
}
section_static_map_ = telemetry_->section("sensors.staticMap");
section_laser_ = telemetry_->section("sensors.laserScan");
section_cloud_ = telemetry_->section("sensors.pointCloud");
section_depth_ = telemetry_->section("sensors.depthCamera");
}
void SensorGateway::attach(robot_costmap_2d::LayeredCostmap* global,
robot_costmap_2d::LayeredCostmap* local)
{
@@ -167,9 +180,9 @@ void SensorGateway::warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap*
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",
"[SensorGateway] costmap %s: layer '%s' of type ObstacleLayer will NOT receive sensor "
"data — this gateway pushes obstacles as LayerType::VOXEL_LAYER. Switch it to 'type: "
"VoxelLayer' in the plugins list if that layer needs data.\n",
which, layer->getName().c_str());
}
}
@@ -241,7 +254,7 @@ void dispatchTo(robot_costmap_2d::LayeredCostmap* costmap, const T& value,
++stats.layer_exceptions;
robot::log_error_throttle(
kHotPathLogThrottle,
"[SensorGateway] layer '%s' (%s) ném exception khi nhận '%s': %s\n",
"[SensorGateway] layer '%s' (%s) threw an exception while taking '%s': %s\n",
layer->getName().c_str(), toString(type), name.c_str(), ex.what());
}
}
@@ -255,11 +268,12 @@ void SensorGateway::pushStaticMap(const std::string& name, const robot_nav_msgs:
{
++stats_.dropped_no_costmap;
robot::log_warning_throttle(kHotPathLogThrottle,
"[SensorGateway] bỏ static map '%s': chưa gắn costmap nào\n",
"[SensorGateway] dropping static map '%s': no costmap attached\n",
name.c_str());
return;
}
ScopedSection timer(telemetry_, section_static_map_);
dispatchTo(global_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
dispatchTo(local_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_);
}
@@ -270,11 +284,12 @@ void SensorGateway::pushLaserScan(const std::string& name, const robot_sensor_ms
{
++stats_.dropped_no_costmap;
robot::log_warning_throttle(kHotPathLogThrottle,
"[SensorGateway] bỏ laser scan '%s': chưa gắn costmap nào\n",
"[SensorGateway] dropping laser scan '%s': no costmap attached\n",
name.c_str());
return;
}
ScopedSection timer(telemetry_, section_laser_);
dispatchTo(local_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
dispatchTo(global_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
}
@@ -286,11 +301,12 @@ void SensorGateway::pushPointCloud(const std::string& name,
{
++stats_.dropped_no_costmap;
robot::log_warning_throttle(kHotPathLogThrottle,
"[SensorGateway] bỏ point cloud '%s': chưa gắn costmap nào\n",
"[SensorGateway] dropping point cloud '%s': no costmap attached\n",
name.c_str());
return;
}
ScopedSection timer(telemetry_, section_cloud_);
dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
}
@@ -302,11 +318,12 @@ void SensorGateway::pushPointCloud2(const std::string& name,
{
++stats_.dropped_no_costmap;
robot::log_warning_throttle(kHotPathLogThrottle,
"[SensorGateway] bỏ point cloud2 '%s': chưa gắn costmap nào\n",
"[SensorGateway] dropping point cloud2 '%s': no costmap attached\n",
name.c_str());
return;
}
ScopedSection timer(telemetry_, section_cloud_);
dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_);
}
@@ -323,11 +340,13 @@ void SensorGateway::pushDepthCameraData(const std::string& topic,
{
++stats_.dropped_no_costmap;
robot::log_warning_throttle(kHotPathLogThrottle,
"[SensorGateway] bỏ depth camera '%s': chưa gắn costmap nào\n",
"[SensorGateway] dropping depth camera '%s': no costmap attached\n",
topic.c_str());
return;
}
ScopedSection timer(telemetry_, section_depth_);
// 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_);

196
src/navigation_runtime.cpp
View File

@@ -12,11 +12,48 @@
#include <robot/node_handle.h>
#include <robot/robot.h>
#include <robot_nav_2d_utils/conversions.h>
#include <tf3/buffer_core.h>
namespace move_base2
{
bool CostmapPosePort::lookupPose(const std::string& frame,
robot_geometry_msgs::PoseStamped& pose) const
{
if (!tf_ || costmap_ == nullptr || frame.empty())
{
return false;
}
// Quy về global frame CỦA COSTMAP, không phải một frame cố định: planner làm việc trên đúng lưới
// đó. Trả pose ở hệ khác là planner nhận toạ độ vô nghĩa mà không tầng nào báo lỗi — đúng lỗi
// "pose start sai frame" đã xảy ra ngày 2026-07-29.
const std::string& target = costmap_->getGlobalFrameID();
try
{
const tf3::TransformStampedMsg tf = tf_->lookupTransform(target, frame, tf3::Time());
pose.header.stamp = robot::Time(tf.header.stamp.sec, tf.header.stamp.nsec);
pose.header.frame_id = target;
pose.pose.position.x = tf.transform.translation.x;
pose.pose.position.y = tf.transform.translation.y;
pose.pose.position.z = tf.transform.translation.z;
pose.pose.orientation.x = tf.transform.rotation.x;
pose.pose.orientation.y = tf.transform.rotation.y;
pose.pose.orientation.z = tf.transform.rotation.z;
pose.pose.orientation.w = tf.transform.rotation.w;
return true;
}
catch (const std::exception& ex)
{
robot::log_warning("[move_base2] cannot resolve frame '%s' in '%s': %s\n", frame.c_str(),
target.c_str(), ex.what());
return false;
}
}
NavigationRuntime::NavigationRuntime() = default;
NavigationRuntime::~NavigationRuntime()
@@ -31,13 +68,13 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
{
if (built_ || costmapsReady())
{
error = "NavigationRuntime::buildCostmaps() gọi lần thứ hai";
error = "NavigationRuntime::buildCostmaps() called twice";
return false;
}
if (!tf)
{
error = "NavigationRuntime cần TF buffer khác null";
error = "NavigationRuntime needs a non-null TF buffer";
return false;
}
@@ -51,14 +88,24 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
//
// Dựng nhưng CHƯA start: thread cập nhật chạy trong lúc 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. start() nằm ở hàm riêng, gọi sau khi lắp xong.
// Telemetry dựng NGAY SAU config và TRƯỚC costmap: costmap tạo thread cập nhật ngay trong
// constructor của nó và không phơi tid ra, nên cách duy nhất gọi đúng tên thread đó mà không phải
// sửa gói costmap là chụp danh sách tid quanh lúc dựng.
stats_.reset(new RuntimeStats(config_.runtime_stats_period));
try
{
stats_->beginThreadCapture();
global_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_));
stats_->endThreadCapture("costmap/global");
stats_->beginThreadCapture();
local_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_));
stats_->endThreadCapture("costmap/local");
}
catch (const std::exception& ex)
{
error = std::string("không dựng được costmap: ") + ex.what();
error = std::string("could not build costmap: ") + ex.what();
global_costmap_.reset();
local_costmap_.reset();
return false;
@@ -67,16 +114,34 @@ bool NavigationRuntime::buildCostmaps(const std::shared_ptr<tf3::BufferCore>& tf
global_costmap_->pause();
local_costmap_->pause();
// Thread update chưa chạy ở pha này, nên chụp footprint config ban đầu ở đây an toàn. Lưu riêng
// hai bản vì runtime phải rollback cả cặp nếu controller không nhận được footprint mới.
global_footprint_ = global_costmap_->getUnpaddedRobotFootprint();
local_footprint_ = local_costmap_->getUnpaddedRobotFootprint();
// Hai nguồn pose, khác frame — xem doc của thành viên. Bản cũ cũng vậy: `makePlan` lấy start từ
// `planner_costmap_robot_` (map), còn `LocalPlannerAdapter` lấy pose từ costmap local (odom).
global_pose_.setCostmap(global_costmap_.get());
local_pose_.setCostmap(local_costmap_.get());
// Chặng có `goal_frame` tra TF qua cổng pose. Chỉ cổng GLOBAL cần: goal của chặng luôn được quy
// về frame của costmap lập plan.
global_pose_.setTf(tf_);
// Costmap ĐIỀU KHIỂN (local) là nguồn của guard "không đi mù" — xem doc của CostmapStatusAdapter.
costmap_status_.setCostmap(local_costmap_.get());
// Gắn ngay: host có thể hỏi dữ liệu hiển thị bất cứ lúc nào sau initialize(), kể cả trước khi
// costmap có nội dung. Exporter tự trả về "chưa có gì" thay vì lưới rỗng.
global_exporter_.attach(global_costmap_.get(), config_.global_frame);
local_exporter_.attach(local_costmap_.get(), local_costmap_->getGlobalFrameID());
// Cả hai exporter dùng chung một tên đoạn công việc: bảng hiện TỔNG chi phí kết xuất cho rviz.
// Tách riêng global/local không nói thêm được gì — host gọi chúng từ các ros::Timer khác nhau và
// câu hỏi cần trả lời là "hiển thị tốn bao nhiêu", không phải "lưới nào tốn hơn".
global_exporter_.attachTelemetry(stats_.get());
local_exporter_.attachTelemetry(stats_.get());
return true;
}
@@ -84,18 +149,24 @@ bool NavigationRuntime::buildRunners(std::string& error)
{
if (built_)
{
error = "NavigationRuntime::buildRunners() gọi lần thứ hai";
error = "NavigationRuntime::buildRunners() called twice";
return false;
}
if (!costmapsReady())
{
error = "buildRunners() gọi trước buildCostmaps()";
error = "buildRunners() called before buildCostmaps()";
return false;
}
robot::NodeHandle root_nh("~");
// --- 3. Planner và controller ------------------------------------------------------------------
//
// Gắn telemetry TRƯỚC configure: PlannerRunner khởi động thread lập plan ngay trong configure(),
// và chính thread đó tự đăng ký nhãn của mình khi bắt đầu chạy.
planner_.attachStats(stats_.get());
controller_.attachStats(stats_.get());
if (!planner_.configure(root_nh, global_costmap_.get(), config_.position.global_planner_name,
error))
{
@@ -126,8 +197,22 @@ bool NavigationRuntime::buildRunners(std::string& error)
// Không dừng lại: một behavior hỏng không nên xoá sạch các đường phục hồi còn lại.
// `behaviorCount()` bên dưới phản ánh số nạp được THẬT, và `validate()` sẽ chặn nếu con số đó
// bằng 0 trong khi recovery vẫn đang bật.
robot::log_warning("[move_base2] NavigationRuntime: có behavior recovery nạp hỏng; chạy tiếp "
"với %zu behavior còn lại.\n", recovery_.behaviorCount());
robot::log_warning("[move_base2] NavigationRuntime: a recovery behavior failed to load; "
"continuing with the remaining %zu behavior(s).\n",
recovery_.behaviorCount());
}
// `recovery_enabled: false` hợp lệ ngay cả khi registry rỗng. Khi đó để StateMachineConfig tự
// validate cặp enabled/count bên dưới; không được ép parse route của một subsystem đã tắt.
if (recovery_.behaviorCount() > 0)
{
std::string recovery_routes_error;
if (!recovery_.configureRoutes(root_nh, recovery_routes_error))
{
error = "invalid recovery routes: " + recovery_routes_error;
return false;
}
config_.state_machine.recovery_routes = recovery_.routes();
}
// Ràng buộc thứ tự khởi tạo — xem doc của lớp. Con số này KHÔNG đến từ YAML.
@@ -137,13 +222,47 @@ bool NavigationRuntime::buildRunners(std::string& error)
action_.setClock(&clock_);
action_.setNamespace(config_.action_namespace);
// Handler dò cần TF để đọc frame thô và GHI lại frame đã lọc — xem action_core::ActionContext.
action_core::ActionContext action_ctx;
action_ctx.tf = tf_.get();
action_ctx.global_frame = global_costmap_->getGlobalFrameID();
action_ctx.robot_base_frame = config_.robot_base_frame;
action_.setContext(action_ctx);
if (!action_.configure(root_nh))
{
robot::log_warning("[move_base2] NavigationRuntime: có action handler nạp hỏng; chạy tiếp với "
"%zu handler còn lại.\n", action_.handlerCount());
robot::log_warning("[move_base2] NavigationRuntime: an action handler failed to load; "
"continuing with the remaining %zu handler(s).\n", action_.handlerCount());
}
// --- 6. Kiểm cấu hình sau cùng -----------------------------------------------------------------
// --- 6. Mission layer --------------------------------------------------------------------------
//
// Không chặn: mission layer hỏng thì navigation vẫn phải chạy được. Nhưng phải LOG rõ, vì hai
// trạng thái này cho hai hành vi khác hẳn nhau với cùng một order — cắt thành chặng, hay đi thẳng
// xuống như một goal duy nhất.
if (config_.mission_layer_enabled)
{
std::string mission_error;
if (mission_layer_.configure(root_nh, config_.mission_namespace, mission_error))
{
mission_layer_.attach(mission_);
robot::log_info("[move_base2] NavigationRuntime: mission layer ready (%zu source(s)) — "
"VDA5050 orders are split into legs.\n", mission_layer_.sourceCount());
}
else
{
robot::log_warning("[move_base2] NavigationRuntime: mission layer NOT started — %s. Orders "
"fall back to the direct path (one goal per order, no leg queue, no "
"released/orderUpdateId handling).\n", mission_error.c_str());
}
}
else
{
robot::log_info("[move_base2] NavigationRuntime: mission layer disabled by config "
"(mission_layer_enabled: false) — orders go straight down as one goal.\n");
}
// --- 7. Kiểm cấu hình sau cùng -----------------------------------------------------------------
if (!config_.validate(error))
{
global_costmap_.reset();
@@ -151,7 +270,7 @@ bool NavigationRuntime::buildRunners(std::string& error)
return false;
}
robot::log_info("[move_base2] NavigationRuntime dựng xong:\n%s", config_.describe().c_str());
robot::log_info("[move_base2] NavigationRuntime built:\n%s", config_.describe().c_str());
built_ = true;
return true;
@@ -165,11 +284,17 @@ void NavigationRuntime::start()
}
global_costmap_->start();
local_costmap_->start();
// Bridge trước layer: executor của layer có thể dispatch ngay ở lần đánh thức đầu tiên, và bridge
// TỪ CHỐI mission khi chưa start (có chủ đích — mission layer phải biết chặng không được nhận).
mission_.start();
mission_layer_.start();
}
void NavigationRuntime::stop()
{
// Ngược chiều dòng dữ liệu: dừng nguồn sinh chặng trước, rồi mới đóng bridge.
mission_layer_.stop();
mission_.stop();
if (local_costmap_)
{
@@ -181,6 +306,54 @@ void NavigationRuntime::stop()
}
}
bool NavigationRuntime::setRobotFootprint(
const std::vector<robot_geometry_msgs::Point>& footprint)
{
if (!costmapsReady())
{
robot::log_error("[move_base2] NavigationRuntime: cannot apply a footprint before both "
"costmaps exist.\n");
return false;
}
// Bản move_base cũ làm đúng hai lời gọi này. Không dùng getCostmap() chung vì mỗi wrapper sở hữu
// padded footprint và phát onFootprintChanged() xuống layer riêng của nó.
global_costmap_->setUnpaddedRobotFootprint(footprint);
local_costmap_->setUnpaddedRobotFootprint(footprint);
// Đây là pha costmap -> runner: chưa có local planner nào để refresh. Nhờ áp footprint tại đây,
// instance planner đầu tiên được dựng phía dưới sẽ snapshot đúng hình robot và không phải dựng
// lại ngay sau startup.
if (!built_)
{
global_footprint_ = footprint;
local_footprint_ = footprint;
robot::log_info("[move_base2] NavigationRuntime: applied initial footprint (%zu point(s)) "
"before planner initialization.\n", footprint.size());
return true;
}
// HybridController (và nhiều planner) snapshot footprint trong initialize(). Recreate instance
// giữ ABI robot_nav_core2 ổn định với plugin .so cũ, đồng thời nạp lại plan đang chạy.
if (!controller_.refreshActivePlanner())
{
// Controller cũ vẫn giữ footprint cũ trong cache. Khôi phục cả hai costmap trước khi trả lỗi
// để không tạo tình trạng global/local/controller dùng ba hình robot khác nhau.
global_costmap_->setUnpaddedRobotFootprint(global_footprint_);
local_costmap_->setUnpaddedRobotFootprint(local_footprint_);
robot::log_error("[move_base2] NavigationRuntime: local planner cache was not refreshed "
"after the footprint changed; both costmaps were rolled back.\n");
return false;
}
global_footprint_ = footprint;
local_footprint_ = footprint;
robot::log_info("[move_base2] NavigationRuntime: propagated footprint (%zu point(s)) to "
"global/local costmaps.\n", footprint.size());
return true;
}
ControlLoopDeps NavigationRuntime::deps()
{
ControlLoopDeps deps;
@@ -196,6 +369,7 @@ ControlLoopDeps NavigationRuntime::deps()
deps.recovery = &recovery_;
deps.mission = &mission_;
deps.action = &action_;
deps.costmap_status = &costmap_status_;
return deps;
}

218
src/navigation_server.cpp
View File

@@ -34,7 +34,7 @@ NavigationServer::NavigationServer()
// 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_->feed_back_str = "not initialized";
nav_feedback_->goal_checked = false;
nav_feedback_->is_ready = false;
}
@@ -60,7 +60,7 @@ bool NavigationServer::startControlThread(double frequency)
{
if (!loop_.initialized())
{
robot::log_error("[move_base2] startControlThread() trước khi control loop được cấu hình.\n");
robot::log_error("[move_base2] startControlThread() before the control loop was configured.\n");
return false;
}
if (control_thread_running_.load())
@@ -69,23 +69,36 @@ bool NavigationServer::startControlThread(double frequency)
}
if (!(frequency > 0.0))
{
robot::log_error("[move_base2] controller_frequency phải > 0 [Hz], nhận %.3f.\n", frequency);
robot::log_error("[move_base2] controller_frequency must be > 0 [Hz], got %.3f.\n", frequency);
return false;
}
control_thread_running_.store(true);
control_thread_ = std::thread([this, frequency]() {
if (stats_ != nullptr)
{
stats_->registerCurrentThread("move_base2/control");
}
robot::Rate rate(frequency);
while (control_thread_running_.load())
{
// Bỏ qua giá trị trả về: false chỉ nghĩa là yêu cầu hiện tại vừa kết thúc, không phải lý do
// dừng vòng lặp — thread phải sống để nhận goal kế tiếp.
spinOnce();
// In bảng thống kê nằm NGOÀI phần đo của cycle: chi phí của chính công cụ đo không được tính
// vào chi phí của runtime, nếu không mỗi lần in lại thành một đỉnh giả trong cột "đỉnh [ms]".
if (stats_ != nullptr)
{
stats_->tick();
}
rate.sleep();
}
});
robot::log_info("[move_base2] control thread chạy ở %.2f Hz.\n", frequency);
robot::log_info("[move_base2] control thread running at %.2f Hz.\n", frequency);
return true;
}
@@ -108,7 +121,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont
if (!loop_.configure(config, deps, error))
{
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "cấu hình lỗi: " + error;
nav_feedback_->feed_back_str = "config error: " + error;
return false;
}
@@ -116,7 +129,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont
nav_feedback_->is_ready = true;
nav_feedback_->feed_back_str = "sẵn sàng";
nav_feedback_->feed_back_str = "ready";
refreshFeedback();
return true;
}
@@ -156,9 +169,14 @@ void NavigationServer::attachCostmaps(robot_costmap_2d::LayeredCostmap* global,
bool NavigationServer::spinOnce()
{
// Bao trọn thân hàm: đây là chi phí một cycle điều khiển, đối chiếu trực tiếp được với
// controller_frequency để biết control loop còn giữ được nhịp hay không.
ScopedSection cycle_timer(stats_, section_cycle_);
// 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ạ.
applyPendingFootprint();
pushHostInputsToController();
drainLifecycleRequests();
@@ -169,9 +187,16 @@ bool NavigationServer::spinOnce()
runtime_->mission().pumpPendingRequest();
}
const bool running = loop_.step();
bool running = false;
{
ScopedSection step_timer(stats_, section_step_);
running = loop_.step();
}
publishCommand();
{
ScopedSection cache_timer(stats_, section_cache_plans_);
cachePlans();
}
refreshFeedback();
return running;
}
@@ -253,9 +278,31 @@ robot::move_base_core::State NavigationServer::toHostState(NavigationState state
return HostState::LOST;
}
bool NavigationServer::missionHasPendingWork() const
{
return runtime_ != nullptr && runtime_->missionLayer().started() &&
runtime_->mission().hasActiveMission();
}
void NavigationServer::refreshFeedback()
{
nav_feedback_->navigation_state = toHostState(loop_.state());
robot::move_base_core::State host_state = toHostState(loop_.state());
// Một order nhiều chặng: lõi về SUCCEEDED sau MỖI chặng, còn order thì chưa xong. Host suy "order
// hoàn thành" thẳng từ SUCCEEDED (amr_vda_5050_client_api.cpp:1062, 1092), nên báo nguyên trạng
// là báo cho fleet master rằng robot đã tới node cuối trong khi nó mới đi được nửa tuyến.
//
// ACTIVE là ánh xạ đúng cho khoảng giữa hai chặng: "yêu cầu đang được xử lý, chưa xong" — và
// KHÔNG phải CONTROLLING, vì robot lúc này đứng yên chờ chặng kế tiếp (host suy `driving` từ
// CONTROLLING, :1233). PENDING cũng phải che: lõi rơi về IDLE trong đúng cycle trước khi chặng
// sau được đẩy xuống.
if (missionHasPendingWork() && (host_state == robot::move_base_core::State::SUCCEEDED ||
host_state == robot::move_base_core::State::PENDING))
{
host_state = robot::move_base_core::State::ACTIVE;
}
nav_feedback_->navigation_state = host_state;
const char* reason = loop_.lastReason();
if (reason != nullptr && reason[0] != '\0')
@@ -293,17 +340,35 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
if (!runtime_->buildCostmaps(tf_, error))
{
runtime_.reset();
stats_ = nullptr;
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không dựng được costmap: " + error;
robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
nav_feedback_->feed_back_str = "could not build costmap: " + error;
robot::log_error("[move_base2] initialize() failed: %s\n", error.c_str());
return;
}
// Telemetry đã tồn tại từ buildCostmaps (nó phải có mặt trước costmap để chụp được thread của
// costmap). Mượn con trỏ và đăng ký các đoạn công việc của chính server ở đây, một lần.
stats_ = runtime_->stats();
if (stats_ != nullptr && stats_->enabled())
{
section_cycle_ = stats_->section("control.cycle");
section_step_ = stats_->section("control.step");
section_cache_plans_ = stats_->section("control.cachePlans");
robot::log_info("[move_base2] telemetry ON — printing stats every %.2f s.\n",
runtime_->config().runtime_stats_period);
}
// Đường nạp cảm biến chạy trên thread callback của host; đo bằng đoạn công việc mới tách được nó
// ra khỏi phần "(không đăng ký)".
sensors_.attachTelemetry(stats_);
if (!configureSensors(runtime_->config().sensors, error))
{
runtime_.reset();
stats_ = nullptr;
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "cấu hình cảm biến lỗi: " + error;
nav_feedback_->feed_back_str = "sensor config error: " + error;
return;
}
@@ -324,20 +389,27 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
attachCostmaps(runtime_->globalCostmap()->getLayeredCostmap(),
runtime_->localCostmap()->getLayeredCostmap());
// Host có thể đã đặt footprint ngay sau khi tạo BaseNavigation nhưng trước initialize(). Áp nó
// vào cả hai costmap TRƯỚC khi dựng runner để local planner đầu tiên snapshot đúng hình robot,
// thay vì khởi tạo với footprint YAML rồi lập tức phải recreate trong cycle đầu tiên.
applyPendingFootprint();
// --- Pha 2: nạp planner, controller, recovery, action ----------------------------------------
if (!runtime_->buildRunners(error))
{
runtime_.reset();
stats_ = nullptr;
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không nạp được runtime: " + error;
robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str());
nav_feedback_->feed_back_str = "could not load runtime: " + error;
robot::log_error("[move_base2] initialize() failed: %s\n", error.c_str());
return;
}
if (!configureLoop(runtime_->config().toControlLoopConfig(), runtime_->deps(), error))
{
runtime_.reset();
robot::log_error("[move_base2] initialize() không cấu hình được control loop: %s\n",
stats_ = nullptr;
robot::log_error("[move_base2] initialize() could not configure the control loop: %s\n",
error.c_str());
return;
}
@@ -349,12 +421,20 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
if (!loop_.submit(request, reason))
{
last_reject_reason_ = reason;
robot::log_error("[move_base2] từ chối chặng mission %llu: %s\n",
robot::log_error("[move_base2] rejecting mission leg %llu: %s\n",
static_cast<unsigned long long>(request.mission_sequence_id),
reason.c_str());
// Bắt buộc báo ngược: chặng bị lõi từ chối sẽ không bao giờ sinh ra outcome theo đường bình
// thường, mà mission layer đã chuyển sang RUNNING lúc giao nó. Im lặng ở đây là cả order treo
// vĩnh viễn ở chặng đó — fleet master chờ một node không bao giờ tới.
runtime_->mission().reportOutcome(request.mission_sequence_id, NavigationOutcome::kFailed);
}
});
runtime_->mission().setCancelCallback([this]() { cancel(); });
// CHỈ huỷ chặng đang chạy: yêu cầu này vừa đi ra từ chính mission layer, gọi cancel() đầy đủ sẽ
// vòng ngược lên xoá hàng đợi của nó.
runtime_->mission().setCancelCallback([this]() { requestLoopCancel(); });
// start() sau cùng: cho thread cập nhật costmap chạy khi mọi thứ khác đã lắp xong.
runtime_->start();
@@ -364,13 +444,14 @@ void NavigationServer::initialize(robot::TFListenerPtr tf)
if (!startControlThread(runtime_->config().controller_frequency))
{
runtime_.reset();
stats_ = nullptr;
nav_feedback_->is_ready = false;
nav_feedback_->feed_back_str = "không khởi động được control thread";
nav_feedback_->feed_back_str = "could not start the control thread";
return;
}
nav_feedback_->is_ready = true;
nav_feedback_->feed_back_str = "sẵn sàng";
nav_feedback_->feed_back_str = "ready";
refreshFeedback();
}
@@ -382,6 +463,7 @@ void NavigationServer::setRobotFootprint(const std::vector<robot_geometry_msgs::
{
std::lock_guard<std::mutex> lock(data_mutex_);
footprint_ = fprt;
footprint_pending_ = true;
}
std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
@@ -390,6 +472,31 @@ std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
return footprint_;
}
void NavigationServer::applyPendingFootprint()
{
if (runtime_ == nullptr)
{
return;
}
std::vector<robot_geometry_msgs::Point> footprint;
{
std::lock_guard<std::mutex> lock(data_mutex_);
if (!footprint_pending_)
{
return;
}
footprint = footprint_;
footprint_pending_ = false;
}
if (!runtime_->setRobotFootprint(footprint))
{
robot::log_error("[move_base2] NavigationServer: footprint update was not fully applied to "
"the navigation runtime.\n");
}
}
// ================================================================================================
// Nhận dữ liệu sensor
// ================================================================================================
@@ -595,18 +702,33 @@ bool NavigationServer::submit(const NavigationRequest& request)
}
last_reject_reason_ = reason;
nav_feedback_->feed_back_str = "từ chối yêu cầu: " + reason;
nav_feedback_->feed_back_str = "request rejected: " + reason;
return false;
}
bool NavigationServer::moveTo(const robot_geometry_msgs::PoseStamped& goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
(void)xy_goal_tolerance;
(void)yaw_goal_tolerance;
// Goal đơn lẻ từ host (RViz /move_base_simple/goal, OPC-UA, ...) cũng là một nguồn mission.
// Nếu bỏ qua GoalSourceAdapter thì request vào ControlLoop có mission_sequence_id == 0 và mất
// lifecycle/cancel thống nhất với VDA5050. GoalSourceAdapter chịu trách nhiệm validate pose, tạo
// SIMPLE_GOAL và gán profile `position`; MissionManager cấp mission id khác 0 trước khi bridge
// giao chặng xuống đây.
//
// Fallback trực tiếp chỉ dành cho cấu hình tương thích cũ: mission layer bị tắt, chưa khởi động,
// hoặc không nạp GoalSourceAdapter. Không được coi false là goal đã được layer nhận.
if (runtime_ != nullptr && runtime_->missionLayer().submitGoal(goal))
{
last_reject_reason_.clear();
return true;
}
NavigationRequest request;
request.profile = MotionProfile::kPosition;
request.goal = goal;
request.tolerance.xy = xy_goal_tolerance;
request.tolerance.yaw = yaw_goal_tolerance;
return submit(request);
}
@@ -614,11 +736,23 @@ bool NavigationServer::moveTo(const robot_protocol_msgs::Order& msg,
const robot_geometry_msgs::PoseStamped& goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
(void)xy_goal_tolerance;
(void)yaw_goal_tolerance;
// Order đi qua mission layer khi layer đang chạy: chỉ ở đó order mới được cắt thành từng chặng
// tại node có action, lọc theo `released`, và nối tiếp được khi fleet master release thêm horizon.
// Đẩy thẳng xuống lõi là dồn cả order thành MỘT goal — action ở node giữa đường không có chỗ chạy.
//
// `submitOrder` trả false khi layer tắt hoặc không có nguồn nhận schema `vda5050.order`; lúc đó
// rơi xuống đường trực tiếp bên dưới, đúng hành vi đã chạy được trên sim.
if (runtime_ != nullptr && runtime_->missionLayer().submitOrder(msg))
{
last_reject_reason_.clear();
return true;
}
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);
}
@@ -627,11 +761,11 @@ bool NavigationServer::dockTo(const std::string& maker,
const robot_geometry_msgs::PoseStamped& goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
(void)xy_goal_tolerance;
(void)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);
}
@@ -640,11 +774,11 @@ bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::
const robot_geometry_msgs::PoseStamped& goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
(void)xy_goal_tolerance;
(void)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);
@@ -653,20 +787,20 @@ bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::
bool NavigationServer::moveStraightTo(const robot_geometry_msgs::PoseStamped& goal,
double xy_goal_tolerance)
{
(void)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)
{
(void)yaw_goal_tolerance;
NavigationRequest request;
request.profile = MotionProfile::kRotate;
request.goal = goal;
request.tolerance.yaw = yaw_goal_tolerance;
return submit(request);
}
@@ -694,6 +828,16 @@ void NavigationServer::resume()
}
void NavigationServer::cancel()
{
std::lock_guard<std::mutex> lock(data_mutex_);
cancel_requested_ = true;
// Huỷ từ host là "bỏ cả order", không chỉ chặng đang chạy: không xoá hàng đợi thì mission layer
// giao chặng kế tiếp ngay sau khi chặng này dừng, và robot chạy tiếp một tuyến vừa bị huỷ.
mission_cancel_requested_ = true;
}
void NavigationServer::requestLoopCancel()
{
std::lock_guard<std::mutex> lock(data_mutex_);
cancel_requested_ = true;
@@ -704,15 +848,18 @@ void NavigationServer::drainLifecycleRequests()
bool pause = false;
bool resume = false;
bool cancel = false;
bool mission_cancel = false;
{
std::lock_guard<std::mutex> lock(data_mutex_);
pause = pause_requested_;
resume = resume_requested_;
cancel = cancel_requested_;
mission_cancel = mission_cancel_requested_;
pause_requested_ = false;
resume_requested_ = false;
cancel_requested_ = false;
mission_cancel_requested_ = false;
}
// Huỷ trước: nó thắng mọi thứ khác. Tạm dừng rồi huỷ và huỷ rồi tạm dừng phải cho cùng kết quả.
@@ -728,6 +875,19 @@ void NavigationServer::drainLifecycleRequests()
{
loop_.requestResume();
}
// Huỷ lan tới cả hàng đợi mission. Đi sau lời gọi xuống lõi vì đây là đường bất đồng bộ (xếp vào
// event bus, xử lý trên thread sự kiện) — lõi phải phản ứng trước, hàng đợi theo sau.
if (mission_cancel && runtime_ != nullptr)
{
runtime_->missionLayer().cancel();
}
// `pause`/`resume` CỐ Ý không lan xuống mission layer. Chặng đang chạy đã bị chính control loop
// giữ lại, nên hàng đợi không giao chặng mới trong lúc tạm dừng dù manager không biết gì.
// Ngược lại, đẩy manager sang PAUSED mở ra một đường hỏng thật: `onNavigationDone` chỉ được nhận
// khi manager đang RUNNING, nên một chặng kết thúc đúng lúc lệnh pause tới sẽ bị **bỏ mất
// outcome** — resume xong cả order treo vĩnh viễn ở chặng đó, không lỗi, không log.
}
bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear)

19
src/navigation_state.cpp
View File

@@ -104,6 +104,25 @@ const char* toString(RecoveryTrigger trigger)
return "unknown";
}
const std::vector<std::size_t>& RecoveryRoutes::forTrigger(RecoveryTrigger trigger) const
{
switch (trigger)
{
case RecoveryTrigger::kPlanningFailed:
return planning_failed;
case RecoveryTrigger::kControllingFailed:
return controlling_failed;
case RecoveryTrigger::kOscillation:
return oscillation;
}
return planning_failed;
}
bool RecoveryRoutes::empty() const
{
return planning_failed.empty() && controlling_failed.empty() && oscillation.empty();
}
const char* toString(RecoveryOutputKind kind)
{
switch (kind)

240
src/runners/action_runner.cpp
View File

@@ -1,15 +1,11 @@
/*********************************************************************
* 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
*********************************************************************/
#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 <string>
#include <robot/robot.h>
@@ -18,11 +14,9 @@ namespace move_base2
ActionRunner::~ActionRunner()
{
// Handler phải chết TRƯỚC factory: factory là thứ giữ .so còn nạp.
// Con trỏ này trỏ vào handler thuộc registry_. Bỏ nó trước khi registry_ bị huỷ để không ai còn
// đường chạm vào một handler đã chết.
active_ = nullptr;
by_type_.clear();
handlers_.clear();
factories_.clear();
}
void ActionRunner::setClock(ClockPort* clock)
@@ -35,199 +29,59 @@ void ActionRunner::setNamespace(const std::string& ns)
namespace_ = ns;
}
bool ActionRunner::registerHandler(const ActionHandler::Ptr& handler)
void ActionRunner::setContext(const action_core::ActionContext& context)
{
if (!handler)
{
robot::log_error("[move_base2] ActionRunner: handler null.");
return false;
context_ = context;
}
const std::vector<std::string> types = handler->supportedActionTypes();
if (types.empty())
bool ActionRunner::registerHandler(const action_core::ActionHandler::Ptr& handler)
{
robot::log_error("[move_base2] ActionRunner: handler không khai actionType nào — sẽ không bao "
"giờ được gọi.");
return false;
return registry_.registerHandler(handler);
}
for (const std::string& type : types)
ActionTick ActionRunner::toTick(const action_core::ActionTick& tick)
{
if (type.empty())
ActionTick out;
out.message = tick.message;
switch (tick.status)
{
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;
}
case action_core::ActionStatus::kRunning:
out.status = ActionTick::Status::kRunning;
break;
case action_core::ActionStatus::kSucceeded:
out.status = ActionTick::Status::kSucceeded;
break;
case action_core::ActionStatus::kFailed:
out.status = ActionTick::Status::kFailed;
break;
}
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;
return out;
}
bool ActionRunner::configure(robot::NodeHandle& nh)
{
if (configured_)
{
robot::log_error("[move_base2] ActionRunner: configure() gọi lần thứ hai.");
robot::log_error("[move_base2] ActionRunner: configure() called twice.");
return false;
}
if (clock_ == nullptr)
{
robot::log_error("[move_base2] ActionRunner: thiếu ClockPort — handler không có mốc timeout.");
// Registry không biết thời gian; handler thì cần mốc để tự timeout. Thiếu đồng hồ là mọi
// handler mất tầng timeout chính của contract.
robot::log_error("[move_base2] ActionRunner: missing ClockPort — handlers would have no "
"timeout reference.");
return false;
}
const std::string key = namespace_.empty() ? std::string("handlers") : namespace_ + "/handlers";
registry_.setContext(context_);
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());
const bool ok = registry_.loadFromConfig(nh, namespace_);
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;
return ok;
}
bool ActionRunner::start(const robot_protocol_msgs::Action& action)
@@ -237,27 +91,40 @@ bool ActionRunner::start(const robot_protocol_msgs::Action& action)
if (!configured_)
{
robot::log_error("[move_base2] ActionRunner: start() trước configure().");
robot::log_error("[move_base2] ActionRunner: start() before configure().");
return false;
}
if (action.actionType.empty())
{
robot::log_error("[move_base2] ActionRunner: action không có actionType.");
robot::log_error("[move_base2] ActionRunner: action has no actionType.");
return false;
}
ActionHandler* handler = find(action.actionType);
action_core::ActionHandler* handler = registry_.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());
// Liệt kê luôn những gì ĐƯỢC nhận. Không có nó, người đọc log mở config ra thấy đúng chữ mình
// vừa gửi (tên instance) và tưởng đã khai rồi — trong khi khoá định tuyến là `action_types`,
// một tên khác nằm ngay bên dưới.
std::string accepted;
for (const std::string& type : registry_.actionTypes())
{
accepted += accepted.empty() ? "" : ", ";
accepted += type;
}
robot::log_error("[move_base2] ActionRunner: no handler accepts actionType '%s' (id '%s'). "
"Accepted actionTypes: [%s]. Lưu ý: khoá định tuyến là `action_types`, không "
"phải tên instance trong `handlers`.",
action.actionType.c_str(), action.actionId.c_str(),
accepted.empty() ? "<none>" : accepted.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').",
robot::log_warning("[move_base2] ActionRunner: handler refused to start action '%s' (id '%s').",
action.actionType.c_str(), action.actionId.c_str());
return false;
}
@@ -269,18 +136,17 @@ bool ActionRunner::start(const robot_protocol_msgs::Action& action)
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.
ActionTick tick;
tick.status = ActionTick::Status::kFailed;
tick.message = "update() khi không có action nào đang chạy";
tick.message = "update() with no action running";
return tick;
}
tick = active_->update(clock_->now());
const ActionTick tick = toTick(active_->update(clock_->now()));
if (tick.status != ActionTick::Status::kRunning)
{

254
src/runners/controller_runner.cpp
View File

@@ -10,6 +10,7 @@
#include <cmath>
#include <exception>
#include <sstream>
#include <utility>
#include <boost/dll/import.hpp>
@@ -39,6 +40,17 @@ bool isFiniteTwist(const robot_geometry_msgs::Twist& twist)
ControllerRunner::ControllerRunner() = default;
ControllerRunner::~ControllerRunner() = default;
void ControllerRunner::attachStats(RuntimeStats* stats)
{
stats_ = stats;
if (stats_ == nullptr)
{
return;
}
section_compute_ = stats_->section("controller.compute");
section_local_plan_ = stats_->section("controller.getLocalPlan");
}
bool ControllerRunner::configure(const robot::NodeHandle& nh,
const std::shared_ptr<tf3::BufferCore>& tf,
robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose,
@@ -46,13 +58,13 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
{
if (configured_)
{
error = "ControllerRunner::configure() gọi lần thứ hai";
error = "ControllerRunner::configure() called twice";
return false;
}
if (costmap == nullptr)
{
error = "ControllerRunner cần costmap local khác null";
error = "ControllerRunner needs a non-null local costmap";
return false;
}
@@ -60,7 +72,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
{
// Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands/isGoalReached; không có nguồn pose
// thì không gọi được hàm nào trong hai hàm đó.
error = "ControllerRunner cần PosePort khác null";
error = "ControllerRunner needs a non-null PosePort";
return false;
}
@@ -72,7 +84,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh,
if (!initial_controller.empty() && !swapPlanner(initial_controller))
{
error = "không nạp được local planner khởi đầu '" + initial_controller + "'";
error = "could not load the initial local planner '" + initial_controller + "'";
configured_ = false;
return false;
}
@@ -85,6 +97,35 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
const auto cached = controllers_.find(name);
if (cached != controllers_.end())
{
if (!marker_dirty_)
{
return cached->second.instance.get();
}
// Marker vừa đổi: instance này có thể đã đọc `maker_name` trong initialize() và không bao giờ
// đọc lại. Dựng lại từ factory sẵn có (không dlopen lại); instance mới thay chỗ instance cũ
// CHỈ khi initialize() thành công — thất bại thì giữ nguyên cache và trả lỗi để bên gọi từ
// chối yêu cầu, không để lại trạng thái nửa vời.
try
{
robot_nav_core2::LocalPlanner::Ptr fresh = cached->second.factory();
if (!fresh)
{
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
"rebuilding for the new marker.\n", name.c_str());
return nullptr;
}
fresh->initialize(nh_, name, tf_, costmap_);
cached->second.instance = std::move(fresh);
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: rebuilding '%s' for the new marker failed: "
"%s\n",
name.c_str(), ex.what());
return nullptr;
}
marker_dirty_ = false;
return cached->second.instance.get();
}
@@ -93,8 +134,8 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& 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",
robot::log_error("[move_base2] ControllerRunner: no library found for '%s' — check the key "
"'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
name.c_str(), name.c_str());
return nullptr;
}
@@ -108,13 +149,13 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const boost::system::system_error& ex)
{
robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%s': %s\n",
robot::log_error("[move_base2] ControllerRunner: could not load symbol '%s' from '%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(),
robot::log_error("[move_base2] ControllerRunner: error while loading '%s': %s\n", name.c_str(),
ex.what());
return nullptr;
}
@@ -125,14 +166,15 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném exception: %s\n",
robot::log_error("[move_base2] ControllerRunner: factory of '%s' threw an 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());
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr.\n",
name.c_str());
return nullptr;
}
@@ -144,15 +186,59 @@ robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n",
robot::log_error("[move_base2] ControllerRunner: initialize() of '%s' threw an exception: %s\n",
name.c_str(), ex.what());
return nullptr;
}
const auto inserted = controllers_.emplace(name, std::move(loaded));
// Instance mới vừa initialize() với `maker_name` hiện hành — marker không còn "chưa được đọc".
marker_dirty_ = false;
return inserted.first->second.instance.get();
}
bool ControllerRunner::setDockingMarker(const std::string& marker)
{
if (!configured_)
{
robot::log_error("[move_base2] ControllerRunner: setDockingMarker() before configure().\n");
return false;
}
// Validate với danh sách `maker_sources` (chuỗi cách nhau bằng space, maker_sources.yaml) —
// đúng phép kiểm bản cũ làm ở cửa dockTo (move_base.cpp:1161-1173). Marker lạ phải bị chặn ở
// đây: để lọt xuống thì getMaker() của docking planner âm thầm không match source nào và robot
// đứng im không lý do.
std::string sources;
nh_.param("maker_sources", sources, std::string(""));
std::stringstream ss(sources);
std::string source;
bool known = false;
while (ss >> source)
{
if (source == marker)
{
known = true;
break;
}
}
if (!known)
{
robot::log_error("[move_base2] ControllerRunner: marker '%s' is not listed in maker_sources "
"('%s').\n", marker.c_str(), sources.c_str());
return false;
}
std::string current;
nh_.param("maker_name", current, std::string(""));
if (current != marker)
{
nh_.setParam("maker_name", marker);
marker_dirty_ = true;
}
return true;
}
void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller)
{
if (controller == nullptr)
@@ -180,7 +266,8 @@ void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* control
}
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());
robot::log_error("[move_base2] ControllerRunner: error while re-applying the velocity limits: "
"%s\n", ex.what());
}
}
@@ -188,19 +275,22 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
{
if (!configured_)
{
robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n");
robot::log_error("[move_base2] ControllerRunner: swapPlanner() before configure().\n");
return false;
}
if (planner_name.empty())
{
robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n");
robot::log_error("[move_base2] ControllerRunner: empty controller name.\n");
return false;
}
if (planner_name == active_name_ && active_ != nullptr)
if (planner_name == active_name_ && active_ != nullptr && !marker_dirty_)
{
return true; // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu.
// Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu. Riêng khi marker vừa đổi
// thì KHÔNG được đi tắt: dock lại cùng planner với marker khác phải rơi xuống acquire() để
// instance được dựng lại và initialize() đọc `maker_name` mới.
return true;
}
robot_nav_core2::LocalPlanner* controller = acquire(planner_name);
@@ -213,33 +303,21 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name)
active_ = controller;
active_name_ = planner_name;
has_active_goal_ = false; // Instance mới chưa biết goal nào.
active_plan_.clear();
applyPendingLimits(active_);
robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n",
robot::log_info("[move_base2] ControllerRunner: active local planner is '%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");
"[move_base2] ControllerRunner: setPlan() with no controller "
"loaded.\n");
return false;
}
@@ -247,7 +325,7 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
{
// 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");
"[move_base2] ControllerRunner: rejecting an empty plan.\n");
return false;
}
@@ -259,7 +337,8 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
if (path.poses.empty())
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: plan chuyển sang Path2D bị rỗng.\n");
"[move_base2] ControllerRunner: plan converted to Path2D came out "
"empty.\n");
return false;
}
@@ -269,17 +348,84 @@ bool ControllerRunner::setPlan(const std::vector<robot_geometry_msgs::PoseStampe
active_->setGoalPose(goal_pose);
active_->setPlan(path);
has_active_goal_ = true;
active_plan_ = plan;
return true;
}
catch (const std::exception& ex)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' ném exception trong setPlan: "
"[move_base2] ControllerRunner: '%s' threw an exception in setPlan: "
"%s\n", active_name_.c_str(), ex.what());
return false;
}
}
bool ControllerRunner::refreshActivePlanner()
{
if (!configured_ || active_ == nullptr)
{
// Chưa có controller active thì không có cache nào cần refresh. Hành vi này giúp footprint có
// thể được host đặt trước goal đầu tiên mà không biến thành lỗi khởi tạo.
return true;
}
const auto loaded = controllers_.find(active_name_);
if (loaded == controllers_.end())
{
robot::log_error("[move_base2] ControllerRunner: active local planner '%s' is absent from "
"the plugin cache.\n", active_name_.c_str());
return false;
}
const bool had_active_goal = has_active_goal_;
const std::vector<robot_geometry_msgs::PoseStamped> saved_plan = active_plan_;
if (had_active_goal && saved_plan.empty())
{
// Không thay instance cũ nếu không thể khôi phục goal đang chạy. Giữ controller hiện tại vẫn
// an toàn hơn việc âm thầm biến navigation thành controller không có plan.
robot::log_error("[move_base2] ControllerRunner: active planner '%s' has a goal but no "
"cached plan to restore after a footprint change.\n", active_name_.c_str());
return false;
}
robot_nav_core2::LocalPlanner::Ptr fresh;
try
{
fresh = loaded->second.factory();
if (!fresh)
{
robot::log_error("[move_base2] ControllerRunner: factory of '%s' returned nullptr while "
"refreshing its footprint cache.\n", active_name_.c_str());
return false;
}
fresh->initialize(nh_, active_name_, tf_, costmap_);
}
catch (const std::exception& ex)
{
// Chỉ thay cache SAU initialize thành công, nên lỗi này không làm mất controller cũ.
robot::log_error("[move_base2] ControllerRunner: refreshing '%s' after a footprint change "
"failed: %s\n", active_name_.c_str(), ex.what());
return false;
}
loaded->second.instance = std::move(fresh);
active_ = loaded->second.instance.get();
has_active_goal_ = false;
active_plan_.clear();
applyPendingLimits(active_);
if (had_active_goal && !setPlan(saved_plan))
{
robot::log_error("[move_base2] ControllerRunner: could not restore the active plan after "
"refreshing '%s' for a footprint change.\n", active_name_.c_str());
return false;
}
robot::log_info("[move_base2] ControllerRunner: refreshed '%s' after the local costmap "
"footprint changed.\n", active_name_.c_str());
return true;
}
bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
{
if (pose_ == nullptr)
@@ -302,8 +448,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
if (!configured_ || active_ == nullptr)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: computeVelocityCommands() khi chưa "
"có controller.\n");
"[move_base2] ControllerRunner: computeVelocityCommands() with no "
"controller loaded.\n");
return false;
}
@@ -317,7 +463,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
if (!currentPose(pose))
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: mất pose, không tính lệnh.\n");
"[move_base2] ControllerRunner: pose lost, not computing a "
"command.\n");
return false;
}
@@ -325,6 +472,9 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
try
{
// Đo đúng lời gọi vào plugin, không đo cả hàm: phần còn lại (tra pose, kiểm NaN) là chi phí của
// move_base2, còn đây mới là chi phí của local planner đang cấu hình.
ScopedSection timer(stats_, section_compute_);
// Gen-2 trả THẲNG lệnh (không có cờ thành công/thất bại) và ném exception khi không tính được —
// ngược với gen-1. Vì vậy nhánh "không có lệnh hợp lệ" ở đây là nhánh catch.
const robot_nav_2d_msgs::Twist2DStamped cmd_2d =
@@ -334,7 +484,7 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
catch (const std::exception& ex)
{
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' không sinh được lệnh: %s\n",
"[move_base2] ControllerRunner: '%s' produced no command: %s\n",
active_name_.c_str(), ex.what());
return false;
}
@@ -344,7 +494,8 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd)
// 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",
"[move_base2] ControllerRunner: '%s' returned a command containing "
"NaN/Inf.\n",
active_name_.c_str());
return false;
}
@@ -378,6 +529,7 @@ bool ControllerRunner::isGoalReached()
if (reached)
{
has_active_goal_ = false;
active_plan_.clear();
}
return reached;
}
@@ -386,7 +538,7 @@ bool ControllerRunner::isGoalReached()
// 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 "
"[move_base2] ControllerRunner: '%s' threw an exception in "
"isGoalReached: %s\n", active_name_.c_str(), ex.what());
return false;
}
@@ -403,6 +555,7 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
try
{
ScopedSection timer(stats_, section_local_plan_);
active_->getPlan(plan);
}
catch (const std::exception& ex)
@@ -410,8 +563,8 @@ void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan)
// Không phải mọi planner đều hỗ trợ; gen-2 cho phép ném. Đây chỉ là dữ liệu hiển thị nên nuốt
// exception là đúng — nhưng vẫn log để không ai tưởng rviz đang hiện quỹ đạo thật.
robot::log_error_throttle(kHotPathLogThrottle,
"[move_base2] ControllerRunner: '%s' không trả được quỹ đạo cục bộ: "
"%s\n", active_name_.c_str(), ex.what());
"[move_base2] ControllerRunner: '%s' could not return a local "
"trajectory: %s\n", active_name_.c_str(), ex.what());
plan = robot_nav_2d_msgs::Path2D();
}
}
@@ -423,7 +576,8 @@ void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& vel
// 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");
"[move_base2] ControllerRunner: dropping a measured velocity "
"containing NaN/Inf.\n");
return;
}
measured_velocity_ = velocity;
@@ -433,7 +587,8 @@ 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");
robot::log_error("[move_base2] ControllerRunner: linear velocity limit contains NaN/Inf, "
"ignored.\n");
return false;
}
@@ -463,7 +618,8 @@ bool ControllerRunner::setTwistLinear(const robot_geometry_msgs::Vector3& linear
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n",
robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistLinear: "
"%s\n",
active_name_.c_str(), ex.what());
return false;
}
@@ -473,7 +629,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
{
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");
robot::log_error("[move_base2] ControllerRunner: angular velocity limit contains NaN/Inf, "
"ignored.\n");
return false;
}
@@ -491,7 +648,8 @@ bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angul
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n",
robot::log_error("[move_base2] ControllerRunner: '%s' threw an exception in setTwistAngular: "
"%s\n",
active_name_.c_str(), ex.what());
return false;
}

62
src/runners/planner_runner.cpp
View File

@@ -52,13 +52,20 @@ PlannerRunner::~PlannerRunner()
}
}
void PlannerRunner::attachStats(RuntimeStats* stats)
{
stats_ = stats;
section_make_plan_ = (stats_ != nullptr) ? stats_->section("planner.makePlan")
: RuntimeStats::kInvalidSection;
}
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";
error = "PlannerRunner::configure() called twice";
return false;
}
@@ -66,7 +73,7 @@ bool PlannerRunner::configure(const robot::NodeHandle& nh,
{
// 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";
error = "PlannerRunner needs a non-null global costmap";
return false;
}
@@ -76,7 +83,7 @@ bool PlannerRunner::configure(const robot::NodeHandle& nh,
if (!initial_planner.empty() && !swapPlanner(initial_planner))
{
error = "không nạp được global planner khởi đầu '" + initial_planner + "'";
error = "could not load the initial global planner '" + initial_planner + "'";
configured_ = false;
return false;
}
@@ -102,8 +109,8 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
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",
robot::log_error("[move_base2] PlannerRunner: no library found for '%s' — check the key "
"'%s/library_path' in the YAML and that the .so file exists in devel/lib.\n",
name.c_str(), name.c_str());
return nullptr;
}
@@ -117,13 +124,14 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
}
catch (const boost::system::system_error& ex)
{
robot::log_error("[move_base2] PlannerRunner: không nạp được symbol '%s' từ '%s': %s\n",
robot::log_error("[move_base2] PlannerRunner: could not load symbol '%s' from '%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());
robot::log_error("[move_base2] PlannerRunner: error while loading '%s': %s\n",
name.c_str(), ex.what());
return nullptr;
}
@@ -133,14 +141,15 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] PlannerRunner: factory của '%s' ném exception: %s\n",
robot::log_error("[move_base2] PlannerRunner: factory of '%s' threw an 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());
robot::log_error("[move_base2] PlannerRunner: factory of '%s' returned nullptr.\n",
name.c_str());
return nullptr;
}
@@ -151,7 +160,7 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] PlannerRunner: initialize() của '%s' ném exception: %s\n",
robot::log_error("[move_base2] PlannerRunner: initialize() of '%s' threw an exception: %s\n",
name.c_str(), ex.what());
return nullptr;
}
@@ -160,7 +169,8 @@ robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& nam
{
// 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());
robot::log_error("[move_base2] PlannerRunner: '%s' reported an initialize() failure.\n",
name.c_str());
return nullptr;
}
@@ -174,13 +184,13 @@ bool PlannerRunner::swapPlanner(const std::string& planner_name)
{
if (!configured_)
{
robot::log_error("[move_base2] PlannerRunner: swapPlanner() trước configure().\n");
robot::log_error("[move_base2] PlannerRunner: swapPlanner() before configure().\n");
return false;
}
if (planner_name.empty())
{
robot::log_error("[move_base2] PlannerRunner: tên planner rỗng.\n");
robot::log_error("[move_base2] PlannerRunner: empty planner name.\n");
return false;
}
@@ -213,7 +223,7 @@ bool PlannerRunner::swapPlanner(const std::string& planner_name)
}
active_name_ = planner_name;
robot::log_info("[move_base2] PlannerRunner: global planner đang dùng là '%s'.\n",
robot::log_info("[move_base2] PlannerRunner: active global planner is '%s'.\n",
planner_name.c_str());
return true;
}
@@ -234,14 +244,14 @@ bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start,
{
if (!configured_)
{
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() trước configure().\n");
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() before 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");
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: start or goal contains NaN/Inf, "
"not starting a planning attempt.\n");
return false;
}
@@ -249,7 +259,8 @@ bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start,
if (active_ == nullptr)
{
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() khi chưa có planner.\n");
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() with no planner "
"loaded.\n");
return false;
}
@@ -317,6 +328,12 @@ void PlannerRunner::cancelPlan()
void PlannerRunner::threadBody()
{
if (stats_ != nullptr)
{
// Phải đăng ký TỪ chính thread này: nhãn được gắn theo tid của thread đang chạy.
stats_->registerCurrentThread("move_base2/planner");
}
std::unique_lock<std::mutex> lock(mutex_);
while (true)
@@ -346,6 +363,7 @@ void PlannerRunner::threadBody()
try
{
ScopedSection timer(stats_, section_make_plan_);
// 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_)
@@ -355,14 +373,14 @@ void PlannerRunner::threadBody()
{
// 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());
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin threw an exception while "
"making a 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");
robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin threw an unknown "
"exception while making a plan.\n");
ok = false;
}

187
src/runners/recovery_runner.cpp
View File

@@ -5,9 +5,12 @@
*********************************************************************/
#include <move_base2/runners/recovery_runner.h>
#include <map>
#include <set>
#include <utility>
#include <robot/robot.h>
#include <yaml-cpp/yaml.h>
namespace move_base2
{
@@ -89,13 +92,13 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
{
if (configured_)
{
robot::log_error("[move_base2] RecoveryRunner: configure() gọi lần thứ hai.");
robot::log_error("[move_base2] RecoveryRunner: configure() called twice.");
return false;
}
if (deps_.clock == nullptr || deps_.pose == nullptr)
{
robot::log_error("[move_base2] RecoveryRunner: thiếu ClockPort hoặc PosePort.");
robot::log_error("[move_base2] RecoveryRunner: missing ClockPort or PosePort.");
return false;
}
@@ -105,8 +108,8 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
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());
robot::log_error("[move_base2] RecoveryRunner: could not load any behavior from namespace '%s' "
"— the runtime will have no recovery path.", namespace_.c_str());
return false;
}
@@ -116,16 +119,170 @@ bool RecoveryRunner::configure(robot::NodeHandle& nh)
{
// 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ỏ.",
robot::log_warning("[move_base2] RecoveryRunner: loaded %zu behavior(s), some entries were "
"dropped.",
registry_.size());
return false;
}
robot::log_info("[move_base2] RecoveryRunner: nạp %zu recovery behavior từ '%s'.",
robot::log_info("[move_base2] RecoveryRunner: loaded %zu recovery behavior(s) from '%s'.",
registry_.size(), namespace_.c_str());
return true;
}
bool RecoveryRunner::configureRoutes(robot::NodeHandle& nh, std::string& error)
{
error.clear();
if (!configured_)
{
error = "configureRoutes() called before a usable recovery registry was configured";
return false;
}
// Luôn dựng fallback trước: schema legacy không có `routes` có nghĩa từng trigger thử toàn bộ
// behavior đã nạp theo đúng thứ tự registry cũ.
routes_ = RecoveryRoutes{};
std::map<std::string, std::size_t> loaded;
for (std::size_t index = 0; index < registry_.size(); ++index)
{
const std::string name = registry_.nameAt(index);
loaded.emplace(name, index);
routes_.planning_failed.push_back(index);
routes_.controlling_failed.push_back(index);
routes_.oscillation.push_back(index);
}
const std::string routes_key = namespace_ + "/routes";
if (!nh.hasParam(routes_key))
{
robot::log_info("[move_base2] RecoveryRunner: '%s' absent; using legacy shared recovery "
"route with %zu behavior(s).",
routes_key.c_str(), registry_.size());
return true;
}
YAML::Node configured_routes;
try
{
configured_routes = nh.getParamValue(routes_key);
}
catch (const YAML::Exception& exception)
{
error = "could not read " + routes_key + ": " + exception.what();
return false;
}
if (!configured_routes.IsMap())
{
error = routes_key + " must be a YAML map";
return false;
}
const std::pair<const char*, RecoveryTrigger> expected_routes[] = {
{"planning_failed", RecoveryTrigger::kPlanningFailed},
{"controlling_failed", RecoveryTrigger::kControllingFailed},
{"oscillation", RecoveryTrigger::kOscillation},
};
const auto is_expected_key = [&expected_routes](const std::string& key) {
for (const auto& expected : expected_routes)
{
if (key == expected.first)
{
return true;
}
}
return false;
};
for (const auto& entry : configured_routes)
{
if (!entry.first.IsScalar())
{
error = routes_key + " contains a non-scalar route name";
return false;
}
const std::string key = entry.first.as<std::string>();
if (!is_expected_key(key))
{
error = routes_key + " has unsupported trigger '" + key + "'";
return false;
}
}
for (const auto& expected : expected_routes)
{
const std::string trigger_name(expected.first);
const YAML::Node route_node = configured_routes[trigger_name];
if (!route_node || !route_node.IsSequence())
{
error = routes_key + "/" + trigger_name + " must be a YAML sequence";
return false;
}
std::vector<std::size_t>* resolved = nullptr;
switch (expected.second)
{
case RecoveryTrigger::kPlanningFailed:
resolved = &routes_.planning_failed;
break;
case RecoveryTrigger::kControllingFailed:
resolved = &routes_.controlling_failed;
break;
case RecoveryTrigger::kOscillation:
resolved = &routes_.oscillation;
break;
}
resolved->clear();
std::set<std::size_t> seen;
for (const YAML::Node& behavior_node : route_node)
{
if (!behavior_node.IsScalar())
{
error = routes_key + "/" + trigger_name + " must contain only behavior names";
return false;
}
const std::string behavior_name = behavior_node.as<std::string>();
const auto found = loaded.find(behavior_name);
if (found == loaded.end())
{
// Đây là điểm cho phép commit schema trước plugin. Không tạo index giả: StateMachine chỉ
// được thấy những index registry thực sự nạp được.
robot::log_warning("[move_base2] RecoveryRunner: route '%s' skips '%s' because that "
"behavior was not loaded.",
trigger_name.c_str(), behavior_name.c_str());
continue;
}
if (!seen.insert(found->second).second)
{
error = routes_key + "/" + trigger_name + " repeats behavior '" + behavior_name + "'";
return false;
}
resolved->push_back(found->second);
}
if (resolved->empty())
{
error = routes_key + "/" + trigger_name +
" has no behavior that was successfully loaded";
return false;
}
}
robot::log_info("[move_base2] RecoveryRunner: routes resolved: planning=%zu controlling=%zu "
"oscillation=%zu.",
routes_.planning_failed.size(), routes_.controlling_failed.size(),
routes_.oscillation.size());
return true;
}
const RecoveryRoutes& RecoveryRunner::routes() const
{
return routes_;
}
std::size_t RecoveryRunner::behaviorCount() const
{
return registry_.size();
@@ -149,15 +306,15 @@ bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger)
if (!configured_)
{
robot::log_error("[move_base2] RecoveryRunner: start() trước configure().");
robot::log_error("[move_base2] RecoveryRunner: start() before 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());
robot::log_error("[move_base2] RecoveryRunner: index %zu out of range (%zu behavior(s)).",
index, registry_.size());
return false;
}
@@ -170,12 +327,20 @@ bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger)
if (!behavior->start(goal, deps_.clock->now()))
{
robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' từ chối khởi động (%s).",
robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' refused to start (%s).",
registry_.nameAt(index).c_str(), toString(trigger));
return false;
}
active_ = behavior;
// Log tại SƯỜN (một dòng cho mỗi lượt khởi động, không nằm trên đường tick). Không có dòng này
// thì trên robot/sim không cách nào biết đang chạy behavior nào: các plugin chỉ log khi chúng TỪ
// CHỐI, nên một chuỗi recovery chạy trơn tru sẽ hoàn toàn im lặng và người vận hành chỉ thấy robot
// tự nhiên quay hoặc lùi.
robot::log_info("[move_base2] recovery: running '%s' (registry index %zu/%zu, trigger %s).",
registry_.nameAt(index).c_str(), index + 1, registry_.size(), toString(trigger));
return true;
}
@@ -188,7 +353,7 @@ RecoveryTick RecoveryRunner::update()
// 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";
tick.message = "update() with no behavior running";
return tick;
}

182
src/state_machine.cpp
View File

@@ -8,6 +8,7 @@
*********************************************************************/
#include <move_base2/core/state_machine.h>
#include <algorithm>
#include <sstream>
namespace move_base2
@@ -23,13 +24,13 @@ bool StateMachineConfig::validate(std::string& error) const
// 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]";
error = "oscillation_distance must be >= 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";
error = "oscillation_timeout is enabled so oscillation_distance must be > 0 [m], otherwise "
"every cycle counts as oscillating";
return false;
}
if (planner_patience <= 0.0 && max_planning_retries < 0)
@@ -38,18 +39,36 @@ bool StateMachineConfig::validate(std::string& error) const
// 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";
error = "planner_patience <= 0 [s] and max_planning_retries < 0 at the same time: nothing can "
"detect a hung planner; set at least one of them";
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";
error = "recovery_enabled = true but recovery_behavior_count = 0; set recovery_enabled = false "
"if you really do not want recovery";
return false;
}
if (!recovery_routes.empty())
{
const auto valid_route = [this](const std::vector<std::size_t>& route) {
return !route.empty() &&
std::all_of(route.begin(), route.end(), [this](std::size_t index) {
return index < recovery_behavior_count;
});
};
if (!valid_route(recovery_routes.planning_failed) ||
!valid_route(recovery_routes.controlling_failed) ||
!valid_route(recovery_routes.oscillation))
{
error = "every configured recovery route must be non-empty and reference a loaded behavior";
return false;
}
}
return true;
}
@@ -58,18 +77,28 @@ std::string StateMachineConfig::describe() const
std::ostringstream out;
out << "StateMachineConfig:\n";
out << " planner_patience : " << planner_patience << " s"
<< (planner_patience > 0.0 ? "" : " (tắt)") << '\n';
<< (planner_patience > 0.0 ? "" : " (off)") << '\n';
out << " controller_patience : " << controller_patience << " s"
<< (controller_patience > 0.0 ? "" : " (tắt)") << '\n';
<< (controller_patience > 0.0 ? "" : " (off)") << '\n';
out << " oscillation_timeout : " << oscillation_timeout << " s"
<< (oscillation_timeout > 0.0 ? "" : " (tắt)") << '\n';
<< (oscillation_timeout > 0.0 ? "" : " (off)") << '\n';
out << " action_patience : " << action_patience << " s"
<< (action_patience > 0.0 ? "" : " (tắt — handler tự timeout)") << '\n';
<< (action_patience > 0.0 ? "" : " (off — handler times out on its own)") << '\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';
<< (max_planning_retries < 0 ? " (unlimited)" : "") << '\n';
out << " recovery_enabled : " << (recovery_enabled ? "true" : "false") << '\n';
out << " recovery_behavior_cnt : " << recovery_behavior_count << '\n';
if (recovery_routes.empty())
{
out << " recovery_routes : legacy shared list\n";
}
else
{
out << " recovery_routes : planning=" << recovery_routes.planning_failed.size()
<< " controlling=" << recovery_routes.controlling_failed.size()
<< " oscillation=" << recovery_routes.oscillation.size() << '\n';
}
return out.str();
}
@@ -79,13 +108,27 @@ std::string StateMachineConfig::describe() const
bool StateMachine::configure(const StateMachineConfig& config, std::string& error)
{
if (!config.validate(error))
config_ = config;
// Schema cũ không có `recovery/routes`: giữ nguyên một list chung cho mọi trigger. Normalise ở
// đây, sau khi RecoveryRunner đã báo số plugin nạp được thật, để phần còn lại của state machine
// chỉ xử lý route đã resolve.
if (config_.recovery_routes.empty())
{
for (std::size_t index = 0; index < config_.recovery_behavior_count; ++index)
{
config_.recovery_routes.planning_failed.push_back(index);
config_.recovery_routes.controlling_failed.push_back(index);
config_.recovery_routes.oscillation.push_back(index);
}
}
if (!config_.validate(error))
{
initialized_ = false;
return false;
}
config_ = config;
initialized_ = true;
reset();
return true;
@@ -100,6 +143,8 @@ void StateMachine::reset()
last_valid_control_ = robot::Time();
last_oscillation_reset_ = robot::Time();
recovery_index_ = 0;
active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
recovery_route_cursors_.fill(0);
planning_retries_ = 0;
request_has_goal_ = true;
action_count_ = 0;
@@ -131,16 +176,49 @@ void StateMachine::beginPlanningCycle(const robot::Time& now)
planning_retries_ = 0;
}
std::size_t& StateMachine::recoveryRouteCursor(RecoveryTrigger trigger)
{
switch (trigger)
{
case RecoveryTrigger::kPlanningFailed:
return recovery_route_cursors_[0];
case RecoveryTrigger::kControllingFailed:
return recovery_route_cursors_[1];
case RecoveryTrigger::kOscillation:
return recovery_route_cursors_[2];
}
return recovery_route_cursors_[0];
}
const std::size_t& StateMachine::recoveryRouteCursor(RecoveryTrigger trigger) const
{
switch (trigger)
{
case RecoveryTrigger::kPlanningFailed:
return recovery_route_cursors_[0];
case RecoveryTrigger::kControllingFailed:
return recovery_route_cursors_[1];
case RecoveryTrigger::kOscillation:
return recovery_route_cursors_[2];
}
return recovery_route_cursors_[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)
const std::vector<std::size_t>& route = config_.recovery_routes.forTrigger(trigger);
const std::size_t cursor = recoveryRouteCursor(trigger);
if (!config_.recovery_enabled || cursor >= route.size())
{
finish(NavigationState::kAborted, NavigationOutcome::kFailed, now,
"hết recovery behavior khả dụng", out);
"no recovery behavior left", out);
return;
}
active_recovery_trigger_ = trigger;
recovery_index_ = route[cursor];
out.start_recovery = true;
out.recovery_index = recovery_index_;
out.recovery_trigger = trigger;
@@ -162,6 +240,8 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
{
out.accept_request = true;
recovery_index_ = 0;
active_recovery_trigger_ = RecoveryTrigger::kPlanningFailed;
recovery_route_cursors_.fill(0);
request_has_goal_ = in.pending_request_has_goal;
action_count_ = in.pending_request_action_count;
action_index_ = 0;
@@ -174,13 +254,13 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
// 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);
"request has neither goal nor action", out);
return;
}
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);
enter(NavigationState::kExecutingActions, in.now, "action-only request", out);
return;
}
@@ -189,7 +269,7 @@ void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachin
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);
enter(NavigationState::kPlanning, in.now, "new request accepted", out);
}
bool StateMachine::preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out)
@@ -231,7 +311,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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";
out.reason = "not configured";
return out;
}
@@ -239,7 +319,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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);
enter(NavigationState::kIdle, in.now, "request finished", 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
@@ -266,14 +346,14 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.cancel_requested)
{
out.stop_planner = true;
enter(NavigationState::kCancelling, in.now, "huỷ khi đang lập plan", out);
enter(NavigationState::kCancelling, in.now, "cancelled while planning", 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);
enter(NavigationState::kPaused, in.now, "paused while planning", out);
break;
}
@@ -294,7 +374,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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);
enter(NavigationState::kControlling, in.now, "valid plan available", out);
break;
}
@@ -313,7 +393,8 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
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);
retries_exhausted ? "planning retries exhausted" : "planning timed "
"out", out);
break;
}
@@ -327,14 +408,14 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.cancel_requested)
{
out.stop_planner = true;
enter(NavigationState::kCancelling, in.now, "huỷ khi đang bám plan", out);
enter(NavigationState::kCancelling, in.now, "cancelled while following the 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);
enter(NavigationState::kPaused, in.now, "paused while following the plan", out);
break;
}
@@ -369,10 +450,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
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);
enter(NavigationState::kExecutingActions, in.now, "goal reached, actions still to run",
out);
break;
}
finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "đạt goal", out);
finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "goal reached",
out);
break;
}
@@ -384,7 +467,8 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
(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);
escalateToRecovery(RecoveryTrigger::kOscillation, in.now, "oscillating in place too long",
out);
break;
}
@@ -398,7 +482,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
out.stop_planner = true;
escalateToRecovery(RecoveryTrigger::kControllingFailed, in.now,
"quá hạn sinh lệnh vận tốc", out);
"velocity command generation timed out", out);
break;
}
@@ -408,7 +492,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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",
enter(NavigationState::kPlanning, in.now, "controller produced no command, replanning",
out);
break;
}
@@ -434,7 +518,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.cancel_requested)
{
out.cancel_recovery = true;
enter(NavigationState::kCancelling, in.now, "huỷ khi đang recovery", out);
enter(NavigationState::kCancelling, in.now, "cancelled while recovering", out);
break;
}
if (in.pause_requested)
@@ -443,7 +527,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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);
enter(NavigationState::kPaused, in.now, "paused while recovering", out);
break;
}
@@ -451,13 +535,17 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
// 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_;
std::size_t& cursor = recoveryRouteCursor(active_recovery_trigger_);
++cursor;
const std::vector<std::size_t>& route =
config_.recovery_routes.forTrigger(active_recovery_trigger_);
recovery_index_ = cursor < route.size() ? route[cursor] : config_.recovery_behavior_count;
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",
in.recovery == RecoveryFeedback::kSucceeded ? "recovery finished, replanning"
: "recovery failed, replanning",
out);
break;
}
@@ -479,7 +567,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.cancel_requested)
{
out.cancel_action = true;
enter(NavigationState::kCancelling, in.now, "huỷ khi đang chạy action", out);
enter(NavigationState::kCancelling, in.now, "cancelled while running an action", out);
break;
}
if (in.pause_requested)
@@ -488,7 +576,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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);
enter(NavigationState::kPaused, in.now, "paused while running an action", out);
break;
}
@@ -497,7 +585,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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",
finish(NavigationState::kAborted, NavigationOutcome::kFailed, in.now, "action failed",
out);
break;
}
@@ -513,7 +601,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
break; // Vẫn ở kExecutingActions, chuyển sang action kế tiếp.
}
finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now,
"action cuối đã xong", out);
"last action finished", out);
break;
}
@@ -524,7 +612,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
{
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);
"action exceeded action_patience", out);
break;
}
@@ -546,7 +634,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.cancel_requested)
{
enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out);
enter(NavigationState::kCancelling, in.now, "cancelled while paused", out);
break;
}
if (in.resume_requested)
@@ -561,7 +649,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (state_before_pause_ == NavigationState::kControlling)
{
out.run_controller = true;
enter(NavigationState::kControlling, in.now, "tiếp tục bám plan", out);
enter(NavigationState::kControlling, in.now, "resume following the plan", out);
}
else if (state_before_pause_ == NavigationState::kExecutingActions)
{
@@ -570,12 +658,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
// 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);
enter(NavigationState::kExecutingActions, in.now, "resume running the action", out);
}
else
{
out.start_planner = true;
enter(NavigationState::kPlanning, in.now, "tiếp tục lập plan", out);
enter(NavigationState::kPlanning, in.now, "resume planning", out);
}
}
break;
@@ -589,7 +677,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in)
if (in.robot_stopped)
{
finish(NavigationState::kCancelled, NavigationOutcome::kCancelled, in.now,
"robot đã dừng hẳn", out);
"robot came to a full stop", out);
}
break;
}

19
src/velocity_arbiter.cpp
View File

@@ -49,32 +49,33 @@ bool VelocityLimits::validate(std::string& error) const
{
if (!(max_vel_x > 0.0))
{
error = "max_vel_x phải > 0 [m/s]";
error = "max_vel_x must be > 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";
error = "min_vel_x is the REVERSE speed limit so it must be <= 0 [m/s]; set 0 to forbid "
"reversing";
return false;
}
if (!(max_vel_theta > 0.0))
{
error = "max_vel_theta phải > 0 [rad/s]";
error = "max_vel_theta must be > 0 [rad/s]";
return false;
}
if (!(max_accel_x > 0.0))
{
error = "max_accel_x phải > 0 [m/s^2]";
error = "max_accel_x must be > 0 [m/s^2]";
return false;
}
if (!(max_accel_theta > 0.0))
{
error = "max_accel_theta phải > 0 [rad/s^2]";
error = "max_accel_theta must be > 0 [rad/s^2]";
return false;
}
if (zero_velocity_epsilon < 0.0)
{
error = "zero_velocity_epsilon phải >= 0";
error = "zero_velocity_epsilon must be >= 0";
return false;
}
return true;
@@ -84,9 +85,9 @@ 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_x : " << max_vel_x << " m/s (forward)\n";
out << " min_vel_x : " << min_vel_x << " m/s (reverse"
<< (min_vel_x == 0.0 ? ", reversing forbidden" : "") << ")\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";

17
test/action_runner_test.cpp
View File

@@ -13,13 +13,14 @@
#include <robot/node_handle.h>
#include <action_core/action_handler.h>
#include <move_base2/runners/action_runner.h>
#include "fake_ports.h"
namespace
{
using move_base2::ActionHandler;
using action_core::ActionHandler;
using move_base2::ActionRunner;
using move_base2::ActionTick;
using move_base2::testing::FakeClockPort;
@@ -45,7 +46,8 @@ public:
{
}
bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
bool configure(const std::string& name, const action_core::ActionContext& /*ctx*/,
robot::NodeHandle& /*nh*/) override
{
name_ = name;
return configure_ok;
@@ -63,10 +65,10 @@ public:
return start_ok;
}
ActionTick update(const robot::Time& /*now*/) override
action_core::ActionTick update(const robot::Time& /*now*/) override
{
++update_count;
ActionTick tick;
action_core::ActionTick tick;
tick.status = next_status;
return tick;
}
@@ -78,7 +80,7 @@ public:
bool configure_ok = true;
bool start_ok = true;
ActionTick::Status next_status = ActionTick::Status::kRunning;
action_core::ActionStatus next_status = action_core::ActionStatus::kRunning;
int start_count = 0;
int update_count = 0;
@@ -224,7 +226,7 @@ TEST(ActionRunner, TicksUntilHandlerFinishes)
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning);
handler->next_status = ActionTick::Status::kSucceeded;
handler->next_status = action_core::ActionStatus::kSucceeded;
EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded);
// Sau khi kết thúc, action không còn active: tick thêm là lỗi thứ tự gọi, không phải kRunning.
@@ -258,7 +260,8 @@ TEST(ActionRunner, ConfigureRequiresClock)
{
ActionRunner runner; // không setClock
robot::NodeHandle nh;
EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort thì handler không có mốc timeout";
EXPECT_FALSE(runner.configure(nh)) << "missing ClockPort means handlers have no timeout "
"reference";
}
TEST(ActionRunner, EmptyHandlerListIsValid)

93
test/config/move_base2_params.yaml
View File

@@ -4,6 +4,7 @@
# --- Tham số runtime, dùng cho config_validation_test ----------------------------------------
move_base2:
docking_requires_marker: false
controller_frequency: 20.0 # [Hz]
planner_frequency: 0.0 # [Hz] 0 = chỉ lập plan khi cần
planner_timeout: 5.0 # [s]
@@ -33,18 +34,53 @@ move_base2:
recovery_namespace: recovery
action_namespace: actions
mission_namespace: mission_adapters
backup_global_planner: TestBackupGlobalPlanner
position:
base_global_planner: TestGlobalPlanner
base_local_planner: TestLocalPlanner
xy_goal_tolerance: 0.15 # [m]
yaw_goal_tolerance: 0.10 # [rad]
docking:
base_global_planner: TestDockPlanner
base_local_planner: TestLocalPlanner
xy_goal_tolerance: 0.02 # [m] ghép nối cần chính xác hơn nhiều
yaw_goal_tolerance: 0.02 # [rad]
docking_marker_profiles:
trolley:
global_planner: TestTrolleyDockPlanner
local_planner: TestTrolleyDockLocalPlanner
# --- Schema runtime root-profile ---------------------------------------------------------------
# Đây là schema dùng bởi move_base_common_params.yaml của move_base2. Không có adapter gen-1.
root_profiles:
controller_frequency: 30.0
planner_frequency: 0.0
planner_patience: 2.0
controller_patience: 0.033333333
max_planning_retries: 0
recovery_behavior_enabled: true
docking_requires_marker: false
backup_global_planner: TestBackupGlobalPlanner
position:
global_planner: TestGlobalPlanner
local_planner: TestLocalPlanner
docking:
global_planner: TestDockPlanner
local_planner: TestLocalPlanner
docking_marker_profiles:
trolley:
global_planner: TestTrolleyDockPlanner
local_planner: TestTrolleyDockLocalPlanner
go_straight:
global_planner: TestStraightPlanner
local_planner: TestStraightLocalPlanner
rotate:
global_planner: TestRotatePlanner
local_planner: TestRotateLocalPlanner
# --- Cấu hình sai, dùng cho test đường lỗi -----------------------------------------------------
move_base2_bad_frequency:
@@ -77,6 +113,11 @@ recovery:
- {name: wait_short, type: WaitRecovery}
- {name: wait_long, type: WaitRecovery}
routes:
planning_failed: [wait_short]
controlling_failed: [wait_long, wait_short]
oscillation: [wait_long]
wait_short:
wait_duration: 1.0 # [s]
wait_long:
@@ -90,9 +131,40 @@ recovery_missing_library:
behaviors:
- {name: ghost, type: GhostRecovery}
# Mô phỏng config được commit trước plugin DetourPathRecovery. `configure()` báo partial failure,
# nhưng configureRoutes() phải bỏ detour_path thay vì đưa index không tồn tại sang StateMachine.
recovery_missing_detour:
behaviors:
- {name: wait_short, type: WaitRecovery}
- {name: detour_path, type: DetourPathRecovery}
routes:
planning_failed: [wait_short]
controlling_failed: [detour_path, wait_short]
oscillation: [detour_path, wait_short]
# --- Recovery THẬT cho kịch bản có vật cản (RecoveryScenarioDriver) ----------------------------
#
# Namespace riêng, KHÔNG dùng chung với `recovery` ở trên: hai bộ test hỏi hai câu khác nhau. Bộ kia
# kiểm chỗ nối RecoveryPort <-> recovery_core bằng behavior họ kNone; bộ này kiểm hành vi an toàn
# THẬT của BackUpRecovery trên lưới có vật cản.
recovery_scenario:
behaviors:
- {name: back_up, type: BackUpRecovery}
back_up:
backup_distance: 0.50 # [m] quãng lùi mong muốn
backup_distance_max: 1.0 # [m] trần cứng
linear_speed: 0.15 # [m/s] độ lớn; dấu âm do plugin đặt (lùi)
acc_lim_x: 1.0 # [m/s^2]
timeout: 15.0 # [s]
WaitRecovery:
library_path: librecovery_core_wait_recovery
BackUpRecovery:
library_path: librecovery_core_back_up_recovery
# GhostRecovery cố ý KHÔNG khai library_path.
# --- Action handler cho action_runner_test -----------------------------------------------------
@@ -136,7 +208,7 @@ actions_missing_library:
- {name: ghost, type: GhostActionHandler}
NoopActionHandler:
library_path: libmove_base2_noop_action_handler
library_path: libaction_core_noop_action_handler
# --- Global planner giả cho planner_runner_test -------------------------------------------------
#
@@ -167,6 +239,14 @@ TestControllerThrowing:
library_path: libmove_base2_test_local_planner
TestControllerRefusesLimits:
library_path: libmove_base2_test_local_planner
TestControllerMarkerProbe:
library_path: libmove_base2_test_local_planner
TestControllerFootprintProbe:
library_path: libmove_base2_test_local_planner
# Danh sách marker cho test đường docking (setDockingMarker) — format chuỗi cách nhau bằng space,
# đúng như maker_sources.yaml production.
maker_sources: dock_a dock_b
# TestControllerMissing cố ý KHÔNG khai library_path.
@@ -181,9 +261,6 @@ legacy_move_base:
max_planning_retries: 0
recovery_behavior_enabled: true
xy_goal_tolerance: 0.25 # [m] default chung cho cả bốn profile
yaw_goal_tolerance: 0.30 # [rad]
base_global_planner: SBPLLatticePlanner
base_local_planner: LocalPlannerAdapter # phải bị BỎ QUA có log

85
test/config_validation_test.cpp
View File

@@ -113,6 +113,37 @@ TEST(MoveBase2Config, ReadsEveryGroupFromYaml)
EXPECT_EQ(config.global_frame, "map");
EXPECT_EQ(config.robot_base_frame, "base_link");
EXPECT_EQ(config.recovery_namespace, "recovery");
EXPECT_FALSE(config.docking_requires_marker);
}
TEST(MoveBase2Config, MissionLayerIsEnabledByDefault)
{
// Mặc định bật: order VDA5050 đi qua mission layer và được cắt thành chặng. Đổi mặc định này là
// đổi hành vi của mọi order trên robot, nên nó được khoá lại bằng test.
const MoveBase2Config defaults;
EXPECT_TRUE(defaults.mission_layer_enabled);
EXPECT_EQ(defaults.mission_namespace, "mission_adapters");
}
TEST(MoveBase2Config, RejectsEnabledMissionLayerWithoutNamespace)
{
MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
config.mission_layer_enabled = true;
config.mission_namespace.clear();
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("mission_namespace"), std::string::npos) << error;
}
TEST(MoveBase2Config, DisabledMissionLayerDoesNotNeedANamespace)
{
MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2);
config.mission_layer_enabled = false;
config.mission_namespace.clear();
std::string error;
EXPECT_TRUE(config.validate(error)) << error;
}
TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
@@ -121,12 +152,37 @@ TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces)
EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.15);
// Ghép nối cần sai số chặt hơn nhiều — đây chính là thứ sáu entry point cũ khác nhau ở.
EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
EXPECT_DOUBLE_EQ(config.docking.default_xy_tolerance, 0.02);
EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.02);
EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
const auto trolley = config.docking_marker_profiles.find("trolley");
ASSERT_NE(trolley, config.docking_marker_profiles.end());
EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
}
TEST(MoveBase2ConfigRootProfile, LoadsIndependentPlannerPairsWithoutTheLegacyAdapter)
{
robot::NodeHandle root;
robot::NodeHandle profile_nh(root, "root_profiles");
const MoveBase2Config config = MoveBase2Config::load(profile_nh);
EXPECT_EQ(config.position.global_planner_name, "TestGlobalPlanner");
EXPECT_EQ(config.position.local_planner_name, "TestLocalPlanner");
EXPECT_EQ(config.docking.global_planner_name, "TestDockPlanner");
EXPECT_EQ(config.docking.local_planner_name, "TestLocalPlanner");
EXPECT_EQ(config.go_straight.global_planner_name, "TestStraightPlanner");
EXPECT_EQ(config.go_straight.local_planner_name, "TestStraightLocalPlanner");
EXPECT_EQ(config.rotate.global_planner_name, "TestRotatePlanner");
EXPECT_EQ(config.rotate.local_planner_name, "TestRotateLocalPlanner");
EXPECT_EQ(config.backup_global_planner_name, "TestBackupGlobalPlanner");
ASSERT_EQ(config.docking_marker_profiles.size(), 1U);
const auto trolley = config.docking_marker_profiles.find("trolley");
ASSERT_NE(trolley, config.docking_marker_profiles.end());
EXPECT_EQ(trolley->second.global_planner_name, "TestTrolleyDockPlanner");
EXPECT_EQ(trolley->second.local_planner_name, "TestTrolleyDockLocalPlanner");
EXPECT_FALSE(config.docking_requires_marker);
}
TEST(MoveBase2Config, LoadedConfigValidates)
@@ -170,17 +226,7 @@ TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll)
std::string error;
EXPECT_FALSE(config.validate(error))
<< "cấu hình này lúc chạy sẽ từ chối MỌI yêu cầu — phải chặn ngay lúc khởi động";
}
TEST(MoveBase2Config, RejectsNonPositiveToleranceOnConfiguredProfile)
{
MoveBase2Config config = minimalValid();
config.position.default_xy_tolerance = 0.0;
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("xy_goal_tolerance"), std::string::npos);
<< "this config would reject EVERY request at runtime — it must be caught at startup";
}
TEST(MoveBase2Config, PropagatesStateMachineValidationFailure)
@@ -267,9 +313,6 @@ TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile)
MoveBase2Config config;
config.fromLegacyNodeHandle(nh);
EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25);
EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30);
EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25);
}
TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled)
@@ -319,8 +362,10 @@ TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema)
robot::NodeHandle root;
const MoveBase2Config config = MoveBase2Config::load(root);
EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới";
EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc";
EXPECT_EQ(config.robot_base_frame, "base_link") << "picked the gen-1 schema even though the new "
"namespace exists";
EXPECT_TRUE(config.sensors.laser_sor_enabled) << "a key that only exists in the new schema was "
"not read";
}
TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace)

78
test/controller_runner_test.cpp
View File

@@ -153,7 +153,8 @@ TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded)
std::string error;
EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error));
EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
"configured";
}
TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive)
@@ -177,7 +178,8 @@ TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries)
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerOk"));
EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk");
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại controller cũ mà vẫn nạp lại thư viện";
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous controller yet "
"reloaded the library";
}
TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive)
@@ -213,7 +215,7 @@ TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin)
ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.10))); // [m/s]
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "trần vận tốc không tới được plugin";
EXPECT_NEAR(cmd.linear.x, 0.10, 1e-9) << "velocity limit never reached the plugin";
}
TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin)
@@ -245,7 +247,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(runner.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "trần đặt trước khi nạp controller bị mất";
EXPECT_NEAR(cmd.linear.x, 0.08, 1e-9) << "a limit set before the controller was loaded got lost";
}
TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
@@ -261,7 +263,8 @@ TEST(ControllerRunner, LimitIsReappliedAfterSwappingController)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "đổi controller làm mất trần vận tốc đang có hiệu lực";
EXPECT_NEAR(cmd.linear.x, 0.07, 1e-9) << "swapping the controller dropped the active velocity "
"limit";
}
TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
@@ -274,6 +277,26 @@ TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse)
EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10)));
}
TEST(ControllerRunner, RefreshActivePlannerReinitializesItAndRestoresThePlan)
{
// Local planners như HybridController copy footprint vào cache trong initialize(). Refresh phải
// tạo instance mới, nhưng không được làm mất goal/plan giữa mission đang chạy.
Fixture fixture("TestControllerFootprintProbe");
ASSERT_TRUE(fixture.ok()) << fixture.error();
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
robot_geometry_msgs::Twist before;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(before));
ASSERT_GT(before.linear.x, 0.0);
ASSERT_TRUE(fixture.runner_.refreshActivePlanner());
robot_geometry_msgs::Twist after;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(after));
EXPECT_GT(after.linear.x, before.linear.x)
<< "planner was not recreated, or its active goal/plan was not restored";
}
TEST(ControllerRunner, NonFiniteLimitIsRejected)
{
Fixture fixture;
@@ -301,7 +324,8 @@ TEST(ControllerRunner, MeasuredVelocityReachesThePlugin)
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "vận tốc đo được không tới được plugin";
EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.30, 1e-9) << "measured velocity never reached the "
"plugin";
}
TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept)
@@ -373,7 +397,7 @@ TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary)
robot_geometry_msgs::Twist cmd;
EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "lệnh chứa NaN vẫn được ghi ra ngoài";
EXPECT_TRUE(std::isfinite(cmd.linear.x)) << "a command containing NaN was still written out";
}
TEST(ControllerRunner, ExceptionFromThePluginIsContained)
@@ -399,6 +423,46 @@ TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt)
EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9);
}
// ================================================================================================
// setDockingMarker — kênh marker của chặng docking
//
// Docking planner đọc `maker_name` đúng MỘT lần trong initialize() (getMaker). Bản cũ dlopen lại
// planner mỗi lần dock nên luôn thấy giá trị mới; ControllerRunner cache instance nên phải tự dựng
// lại khi marker đổi — không làm là robot dock vào marker của chặng TRƯỚC, lặng lẽ.
// ================================================================================================
TEST(ControllerRunnerDockingMarker, RejectsMarkerNotInMakerSources)
{
Fixture fixture;
ASSERT_TRUE(fixture.ok()) << fixture.error();
EXPECT_FALSE(fixture.runner_.setDockingMarker("khong_ton_tai"));
// Marker hợp lệ (test/config: `maker_sources: dock_a dock_b`) phải qua.
EXPECT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
}
TEST(ControllerRunnerDockingMarker, CachedPlannerIsReinitializedWhenMarkerChanges)
{
Fixture fixture;
ASSERT_TRUE(fixture.ok()) << fixture.error();
ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_a"));
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
robot_geometry_msgs::Twist cmd;
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.11, 1e-9) << "probe could not read maker_name='dock_a' at init";
// Đổi marker rồi swap lại CÙNG planner: instance cache phải được dựng lại để initialize() đọc
// giá trị mới. Nếu không, lệnh vẫn mang mã của dock_a — chính là "dock vào nhầm trạm".
ASSERT_TRUE(fixture.runner_.setDockingMarker("dock_b"));
ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerMarkerProbe"));
ASSERT_TRUE(fixture.runner_.setPlan(makePlan()));
ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd));
EXPECT_NEAR(cmd.linear.x, 0.22, 1e-9)
<< "instance cache kept the old maker_name — it must re-init when the marker changes";
}
int main(int argc, char** argv)
{
setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);

70
test/fake_ports.h
View File

@@ -27,6 +27,7 @@
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/costmap_status_port.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/planner_port.h>
#include <move_base2/ports/pose_port.h>
@@ -173,6 +174,7 @@ public:
++make_plan_count_;
saw_order_ = saw_order_ || order != nullptr;
order_history_.push_back(order != nullptr);
in_flight_ = true;
pending_tag_ = tag;
@@ -276,6 +278,11 @@ public:
return saw_order_;
}
const std::vector<bool>& orderHistory() const
{
return order_history_;
}
private:
/// Hết kịch bản thì giữ kết quả cuối; kịch bản rỗng thì luôn thành công.
PlannerScript nextAction()
@@ -307,6 +314,7 @@ private:
std::size_t make_plan_count_ = 0;
std::size_t swap_count_ = 0;
bool saw_order_ = false;
std::vector<bool> order_history_;
};
// ------------------------------------------------------------------------------------------------
@@ -324,10 +332,21 @@ public:
return true;
}
void setTolerance(double xy_m, double yaw_rad) override
bool setDockingMarker(const std::string& marker) override
{
xy_tolerance_ = xy_m;
yaw_tolerance_ = yaw_rad;
last_docking_marker_ = marker;
return docking_marker_succeeds_;
}
void setDockingMarkerSucceeds(bool succeeds)
{
docking_marker_succeeds_ = succeeds;
}
/// @brief Marker của lời gọi setDockingMarker gần nhất; rỗng nếu chưa từng gọi.
const std::string& lastDockingMarker() const
{
return last_docking_marker_;
}
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override
@@ -472,16 +491,6 @@ public:
return last_plan_size_;
}
double xyTolerance() const
{
return xy_tolerance_;
}
double yawTolerance() const
{
return yaw_tolerance_;
}
private:
robot_nav_2d_msgs::Path2D local_plan_;
robot_geometry_msgs::Twist measured_velocity_;
@@ -509,9 +518,9 @@ private:
std::string active_;
bool swap_succeeds_ = true;
bool set_plan_succeeds_ = true;
bool docking_marker_succeeds_ = true;
std::string last_docking_marker_;
double nominal_speed_ = 0.3; ///< [m/s]
double xy_tolerance_ = 0.0; ///< [m]
double yaw_tolerance_ = 0.0; ///< [rad]
std::size_t set_plan_count_ = 0;
std::size_t compute_count_ = 0;
@@ -814,6 +823,37 @@ private:
// ------------------------------------------------------------------------------------------------
/**
* @class FakeCostmapStatusPort
* @brief Costmap "còn hạn / hết hạn" bật tắt được, cho guard không-đi-mù.
*
* Mặc định **còn hạn**: một cổng giả im lặng chặn robot sẽ làm mọi test khác fail vì lý do không
* liên quan tới thứ chúng đang kiểm.
*/
class FakeCostmapStatusPort final : public CostmapStatusPort
{
public:
bool isCurrent() const override
{
++query_count_;
return current_;
}
void setCurrent(bool current)
{
current_ = current;
}
std::size_t queryCount() const
{
return query_count_;
}
private:
bool current_ = true;
mutable std::size_t query_count_ = 0;
};
class FakeMissionPort final : public MissionPort
{
public:

81
test/mission_adapter_bridge_test.cpp
View File

@@ -96,6 +96,71 @@ TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse)
EXPECT_EQ(request.actions.size(), 3U);
}
TEST(MissionAdapterBridgeConversion, CarriesDockingMarkerIndependentlyOfGoalFrame)
{
auto mission = makeMission(10);
mission->motion_hint = "docking";
mission->marker = "charger";
mission->goal_frame = "charger_02_goal";
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.profile, move_base2::MotionProfile::kDocking);
EXPECT_EQ(request.marker, "charger");
EXPECT_EQ(request.goal_frame, "charger_02_goal");
}
TEST(MissionAdapterBridgeConversion, OrderLegCarriesItsOwnNodesAndEdges)
{
// Global planner của profile position (`CustomPlanner`) CHỈ hiện thực nhánh
// makePlan(Order, ...); nhánh ba tham số của nó là stub trả false. Chặng đi xuống mà không mang
// order thì fail ngay lượt lập plan đầu và chạy thẳng vào recovery cho tới ABORTED — đã xảy ra
// thật ngày 2026-07-31.
auto mission = makeMission(4, 2.0);
mission->type = mission_adapters::MissionType::VDA5050_ORDER;
robot_protocol_msgs::Node n0;
n0.nodeId = "n0";
robot_protocol_msgs::Node n1;
n1.nodeId = "n1";
mission->nodes = { n0, n1 };
robot_protocol_msgs::Edge e0;
e0.edgeId = "e0";
e0.startNodeId = "n0";
e0.endNodeId = "n1";
e0.trajectory.degree = 1;
mission->edges = { e0 };
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
ASSERT_NE(request.order, nullptr);
ASSERT_EQ(request.order->nodes.size(), 2U);
EXPECT_EQ(request.order->nodes[1].nodeId, "n1");
// Edge của CHẶNG, không phải của cả order: planner tra edge theo startNodeId/endNodeId trong tập
// node nó nhận được, nên tập hai bên phải khớp nhau.
ASSERT_EQ(request.order->edges.size(), 1U);
EXPECT_EQ(request.order->edges[0].startNodeId, "n0");
EXPECT_EQ(request.order->edges[0].trajectory.degree, 1U);
}
TEST(MissionAdapterBridgeConversion, SimpleGoalMissionCarriesNoOrder)
{
const auto mission = makeMission(2); // mặc định SIMPLE_GOAL
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.order, nullptr);
}
TEST(MissionAdapterBridgeConversion, ActionOnlyLegCarriesNoOrder)
{
// Không có quãng đường nào để lập plan, nên cũng không có gì để đưa cho planner.
auto mission = makeMission(3, 0.0, 1, /*has_goal=*/false);
mission->type = mission_adapters::MissionType::VDA5050_ORDER;
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_EQ(request.order, nullptr);
}
TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
{
// Quy ước của NavigationRequest: sai số <= 0 nghĩa "dùng default của profile trong config".
@@ -103,8 +168,6 @@ TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault)
const auto mission = makeMission(1);
const NavigationRequest request = MissionAdapterBridge::toRequest(*mission);
EXPECT_FALSE(request.tolerance.hasXy());
EXPECT_FALSE(request.tolerance.hasYaw());
}
// ================================================================================================
@@ -118,7 +181,8 @@ TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped)
Fixture fixture;
ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3)));
EXPECT_TRUE(fixture.received_.empty()) << "dispatch đi thẳng xuống navigation, bỏ qua biên thread";
EXPECT_TRUE(fixture.received_.empty()) << "dispatch went straight down to navigation, skipping "
"the thread boundary";
EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
ASSERT_EQ(fixture.received_.size(), 1U);
@@ -139,7 +203,7 @@ TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce)
ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4)));
EXPECT_TRUE(fixture.bridge_.pumpPendingRequest());
EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "cùng một mission bị đẩy xuống hai lần";
EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "the same mission was pushed down twice";
EXPECT_EQ(fixture.received_.size(), 1U);
}
@@ -192,7 +256,7 @@ TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted)
ASSERT_TRUE(fixture.bridge_.pumpPendingRequest());
ASSERT_EQ(fixture.received_.size(), 1U);
EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "mission cũ thắng mission mới";
EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "the old mission won over the new one";
}
// ================================================================================================
@@ -238,7 +302,8 @@ TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported)
fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded);
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "goal trực tiếp bị đem báo lên mission layer";
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "a direct goal was reported up to the mission "
"layer";
}
TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
@@ -249,13 +314,13 @@ TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone)
manager.submit({ makeMission(0) });
const auto running = manager.nextMission();
ASSERT_TRUE(running) << "manager không giao mission nào để chạy";
ASSERT_TRUE(running) << "manager handed over no mission to run";
fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded);
EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U);
EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId)
<< "mission vẫn còn đang chạy sau khi đã báo hoàn tất";
<< "mission is still running after completion was reported";
}
TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted)

368
test/mission_layer_test.cpp Normal file
View File

@@ -0,0 +1,368 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — test MissionLayer lắp với MissionAdapterBridge thật.
*
* Đây là chỗ kiểm thứ mà `mission_adapter_bridge_test` không kiểm được: bridge có được nối vào một
* mission layer ĐANG CHẠY hay không, và một yêu cầu nhiều chặng có thật sự đi hết chặng này tới
* chặng kia hay không. Bridge đúng mà layer không được dựng thì mọi test bridge vẫn xanh trong khi
* robot chỉ chạy được chặng đầu — đúng trạng thái của gói trước lần sửa này.
*
* Nguồn mission ở đây được đăng ký thẳng vào registry (không qua Boost.DLL): đường nạp `.so` đã có
* `plugin_registry_test` của `mission_adapters` lo, còn thứ cần khoá tại đây là chuỗi sự kiện.
*
* Author: DuongTD
*********************************************************************/
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include <mission_adapters/mission_request.h>
#include <mission_adapters/types.h>
#include <move_base2/bridges/mission_layer.h>
namespace
{
using mission_adapters::ConversionResult;
using mission_adapters::Mission;
using mission_adapters::MissionRequest;
using mission_adapters::MissionSourceAdapter;
using mission_adapters::MissionState;
using mission_adapters::SubmitMode;
using move_base2::MissionAdapterBridge;
using move_base2::MissionLayer;
using move_base2::NavigationOutcome;
using move_base2::NavigationRequest;
constexpr auto kTimeout = std::chrono::seconds(2);
constexpr auto kPollStep = std::chrono::milliseconds(2);
/**
* @brief Nguồn giả cắt một pose thành @c legs chặng, mô phỏng đúng hình dạng của một VDA5050 order
* nhiều node có action.
*/
class SplittingAdapter : public MissionSourceAdapter
{
public:
explicit SplittingAdapter(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
: legs_(legs), mode_(mode)
{
}
bool configure(const std::string&, robot::NodeHandle&) override
{
return true;
}
std::string schema() const override
{
return mission_adapters::schema::kPoseStamped;
}
bool validate(const MissionRequest& request, std::string& reason) const override
{
if (!request.pose)
{
reason = "request carries no pose";
return false;
}
return true;
}
ConversionResult convert(const MissionRequest& request) override
{
ConversionResult result;
result.mode = mode_;
for (std::size_t i = 0; i < legs_; ++i)
{
auto mission = std::make_shared<Mission>();
mission->has_goal = true;
mission->goal = *request.pose;
// Mỗi chặng xa hơn chặng trước một mét — đủ để test phân biệt được chặng nào đang chạy.
mission->goal.pose.position.x = static_cast<double>(i + 1); // [m]
result.missions.push_back(mission);
}
return result;
}
private:
std::size_t legs_;
SubmitMode mode_;
};
robot_geometry_msgs::PoseStamped makeGoal()
{
robot_geometry_msgs::PoseStamped goal;
goal.header.frame_id = "map";
goal.pose.orientation.w = 1.0;
return goal;
}
/// @brief Layer + bridge đã nối, cộng chỗ nhận yêu cầu như control thread thật.
class Fixture
{
public:
explicit Fixture(std::size_t legs, SubmitMode mode = SubmitMode::kReplace)
{
adapter_ = std::make_shared<SplittingAdapter>(legs, mode);
EXPECT_TRUE(layer_.registry().registerAdapter(adapter_));
layer_.markActiveForTesting();
layer_.attach(bridge_);
bridge_.setRequestCallback([this](const NavigationRequest& request) {
received_.push_back(request);
});
bridge_.setCancelCallback([this]() { ++cancel_calls_; });
bridge_.start();
layer_.start();
}
~Fixture()
{
layer_.stop();
bridge_.stop();
}
/**
* @brief Quay control thread cho tới khi một chặng được đẩy xuống, hoặc hết thời gian chờ.
* @return false nếu không có chặng nào tới — dùng để khẳng định "KHÔNG được có chặng mới".
*/
bool pumpUntilRequest()
{
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (std::chrono::steady_clock::now() < deadline)
{
if (bridge_.pumpPendingRequest())
{
return true;
}
std::this_thread::sleep_for(kPollStep);
}
return false;
}
/// @brief Chờ mission layer đạt tới trạng thái mong đợi.
bool waitForState(MissionState expected)
{
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (std::chrono::steady_clock::now() < deadline)
{
if (layer_.state() == expected)
{
return true;
}
std::this_thread::sleep_for(kPollStep);
}
return false;
}
/// @brief Báo kết cục của chặng vừa nhận, đúng như ControlLoop làm ở cuối một phiên.
void finishLastLeg(NavigationOutcome outcome)
{
ASSERT_FALSE(received_.empty());
bridge_.reportOutcome(received_.back().mission_sequence_id, outcome);
}
MissionLayer layer_;
MissionAdapterBridge bridge_;
std::vector<NavigationRequest> received_;
int cancel_calls_ = 0;
std::shared_ptr<SplittingAdapter> adapter_;
};
// ================================================================================================
// Chuỗi nhiều chặng — lý do lớp này tồn tại
// ================================================================================================
TEST(MissionLayerTest, ThreeLegOrderReachesNavigationOneLegAtATime)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
// Chặng 1
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(1u, fixture.received_.size());
EXPECT_DOUBLE_EQ(1.0, fixture.received_[0].goal.pose.position.x);
EXPECT_NE(0u, fixture.received_[0].mission_sequence_id);
// Chặng 2 chỉ được giao SAU khi chặng 1 báo xong: hàng đợi tuần tự, không phải bắn hết một lượt.
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(2u, fixture.received_.size());
EXPECT_DOUBLE_EQ(2.0, fixture.received_[1].goal.pose.position.x);
// Chặng 3
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
ASSERT_EQ(3u, fixture.received_.size());
EXPECT_DOUBLE_EQ(3.0, fixture.received_[2].goal.pose.position.x);
// Mỗi chặng một id riêng: outcome trễ của chặng cũ không thể được tính cho chặng mới.
EXPECT_NE(fixture.received_[0].mission_sequence_id, fixture.received_[1].mission_sequence_id);
EXPECT_NE(fixture.received_[1].mission_sequence_id, fixture.received_[2].mission_sequence_id);
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
EXPECT_FALSE(fixture.layer_.hasMission());
}
TEST(MissionLayerTest, MissionStillPendingBetweenLegs)
{
Fixture fixture(2);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
// Đây là tín hiệu mà NavigationServer dùng để KHÔNG báo SUCCEEDED cho host giữa hai chặng. Sai ở
// đây nghĩa là fleet master nghe "đã tới node cuối" khi robot mới đi được nửa tuyến.
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.bridge_.hasActiveMission());
ASSERT_TRUE(fixture.pumpUntilRequest());
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
EXPECT_TRUE(fixture.waitForState(MissionState::COMPLETED));
EXPECT_FALSE(fixture.bridge_.hasActiveMission());
}
// ================================================================================================
// Đường lỗi và đường huỷ
// ================================================================================================
TEST(MissionLayerTest, FailedLegClearsTheRestOfTheQueue)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
// clear_queue_on_failure mặc định true: không tới được node n thì chạy tiếp chặng n+1 là cắt ngang
// đoạn đường fleet manager chưa cho phép đi.
fixture.finishLastLeg(NavigationOutcome::kFailed);
EXPECT_TRUE(fixture.waitForState(MissionState::FAILED));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
TEST(MissionLayerTest, CancelStopsNavigationAndDropsTheQueue)
{
Fixture fixture(3);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
fixture.layer_.cancel();
EXPECT_TRUE(fixture.waitForState(MissionState::CANCELLED));
// Huỷ phải dừng được robot, không chỉ xoá hàng đợi trong bộ nhớ (A3).
const auto deadline = std::chrono::steady_clock::now() + kTimeout;
while (fixture.cancel_calls_ == 0 && std::chrono::steady_clock::now() < deadline)
{
std::this_thread::sleep_for(kPollStep);
}
EXPECT_GE(fixture.cancel_calls_, 1);
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
TEST(MissionLayerTest, PausedLayerHoldsTheQueueUntilResume)
{
Fixture fixture(2);
fixture.layer_.pause();
ASSERT_TRUE(fixture.waitForState(MissionState::PAUSED));
// Yêu cầu tới trong lúc người vận hành đang chủ động dừng: nhận vào hàng đợi nhưng KHÔNG tự chạy.
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_TRUE(fixture.received_.empty());
fixture.layer_.resume();
EXPECT_TRUE(fixture.pumpUntilRequest());
EXPECT_EQ(1u, fixture.received_.size());
}
// ================================================================================================
// Định tuyến: layer chỉ nhận thứ nó có nguồn để xử lý
// ================================================================================================
TEST(MissionLayerTest, RejectsSchemaWithNoRegisteredSource)
{
Fixture fixture(1);
// Không có nguồn nào khai schema `vda5050.order` — layer phải TỪ CHỐI để NavigationServer biết
// đường mà rơi về nhánh trực tiếp, thay vì nuốt order rồi im lặng.
robot_protocol_msgs::Order order;
EXPECT_FALSE(fixture.layer_.handles(mission_adapters::schema::kVda5050Order));
EXPECT_FALSE(fixture.layer_.submitOrder(order));
}
TEST(MissionLayerTest, RejectsEverythingBeforeStartAndAfterStop)
{
MissionLayer layer;
MissionAdapterBridge bridge;
auto adapter = std::make_shared<SplittingAdapter>(1);
ASSERT_TRUE(layer.registry().registerAdapter(adapter));
layer.markActiveForTesting();
layer.attach(bridge);
// Chưa start: không được nhận việc mà sẽ không ai chạy.
EXPECT_FALSE(layer.submitGoal(makeGoal()));
bridge.start();
layer.start();
EXPECT_TRUE(layer.submitGoal(makeGoal()));
layer.stop();
bridge.stop();
EXPECT_FALSE(layer.submitGoal(makeGoal()));
}
TEST(MissionLayerTest, InactiveLayerNeverStarts)
{
// Không nguồn nào -> configure sẽ hỏng ở runtime thật; ở đây kiểm bất biến tương ứng: layer không
// active thì start() là no-op và mọi đường vào đều đóng.
MissionLayer layer;
MissionAdapterBridge bridge;
layer.attach(bridge);
layer.start();
EXPECT_FALSE(layer.active());
EXPECT_FALSE(layer.started());
EXPECT_EQ(0u, layer.sourceCount());
EXPECT_FALSE(layer.submitGoal(makeGoal()));
}
// ================================================================================================
// Order update — phần release thêm nối tiếp, không chạy lại từ đầu
// ================================================================================================
TEST(MissionLayerTest, AppendModeDoesNotPreemptTheRunningLeg)
{
Fixture fixture(1, SubmitMode::kAppend);
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
ASSERT_TRUE(fixture.pumpUntilRequest());
const std::uint64_t running_id = fixture.received_.back().mission_sequence_id;
// Bản cập nhật tới trong lúc chặng cũ đang chạy: nó phải nằm chờ, không được huỷ chặng đang đi.
ASSERT_TRUE(fixture.layer_.submitGoal(makeGoal()));
EXPECT_FALSE(fixture.pumpUntilRequest());
EXPECT_EQ(0, fixture.cancel_calls_);
fixture.finishLastLeg(NavigationOutcome::kSucceeded);
ASSERT_TRUE(fixture.pumpUntilRequest());
EXPECT_NE(running_id, fixture.received_.back().mission_sequence_id);
}
} // namespace
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

33
test/move_base2_scenario_driver.h
View File

@@ -54,8 +54,8 @@ public:
if (!scenario.obstacles.empty())
{
error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); "
"dùng driver có recovery_core thật cho kịch bản va chạm";
error = "this driver does not simulate obstacles (the recovery port is faked by the "
"scenario); use the driver with the real recovery_core for collision scenarios";
return false;
}
@@ -76,7 +76,7 @@ public:
}
else
{
error = "planner_script không hiểu: '" + item + "'";
error = "unknown planner_script: '" + item + "'";
return false;
}
}
@@ -106,7 +106,7 @@ public:
}
else
{
error = "controller_script không hiểu: '" + item + "'";
error = "unknown controller_script: '" + item + "'";
return false;
}
}
@@ -128,7 +128,7 @@ public:
}
else
{
error = "recovery_script không hiểu: '" + item + "'";
error = "unknown recovery_script: '" + item + "'";
return false;
}
}
@@ -136,9 +136,10 @@ public:
for (const nav_test_harness::ScenarioEvent& event : scenario.events)
{
if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
event.action != "lose_pose" && event.action != "restore_pose")
event.action != "lose_pose" && event.action != "restore_pose" &&
event.action != "sensors_stale" && event.action != "sensors_ok")
{
error = "events: action không hiểu: '" + event.action + "'";
error = "events: unknown action: '" + event.action + "'";
return false;
}
}
@@ -147,6 +148,13 @@ public:
controller_.setScript(controller_script);
recovery_.setScript(recovery_script);
// Recovery thế hệ 2 có thể tự lái. Kịch bản nào khai `recovery_velocity` thì behavior được coi
// là họ velocity; 0 nghĩa là behavior chỉ đợi/xoá costmap và lõi phải giữ nguồn vận tốc kNone.
const bool recovery_drives = std::abs(scenario.recovery_velocity) > 0.0;
recovery_.setRecoveryVelocity(recovery_drives, scenario.recovery_velocity); // [m/s]
recovery_.setDefaultOutputKind(recovery_drives ? RecoveryOutputKind::kVelocity
: RecoveryOutputKind::kNone);
pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
ControlLoopConfig config;
@@ -181,6 +189,7 @@ public:
deps_.recovery = &recovery_;
deps_.mission = &mission_;
deps_.action = &action_;
deps_.costmap_status = &costmap_status_;
if (!loop_.configure(config, deps_, error))
{
@@ -269,6 +278,15 @@ private:
{
pose_.setAvailable(true);
}
else if (event.action == "sensors_stale")
{
// Observation buffer của costmap hết hạn — lõi phải ngừng cho lái bánh xe.
costmap_status_.setCurrent(false);
}
else if (event.action == "sensors_ok")
{
costmap_status_.setCurrent(true);
}
}
}
@@ -283,6 +301,7 @@ private:
FakeRecoveryPort recovery_{ 2 };
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
std::size_t cycle_ = 0;
bool started_ = false;

46
test/move_base2_scenario_test.cpp
View File

@@ -12,6 +12,7 @@
#include <gtest/gtest.h>
#include <algorithm>
#include <cstdlib>
#include <string>
#include <vector>
@@ -19,10 +20,12 @@
#include <nav_test_harness/scenario_runner.h>
#include "move_base2_scenario_driver.h"
#include "recovery_scenario_driver.h"
namespace
{
using move_base2::testing::MoveBase2ScenarioDriver;
using move_base2::testing::RecoveryScenarioDriver;
using nav_test_harness::Scenario;
using nav_test_harness::ScenarioReport;
using nav_test_harness::ScenarioRunner;
@@ -47,13 +50,34 @@ void runScenarioFile(const std::string& path)
Scenario scenario;
std::string error;
ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error))
<< "không nạp được " << path << ": " << error;
MoveBase2ScenarioDriver driver;
ASSERT_TRUE(driver.setup(scenario, error)) << scenario.name << ": setup thất bại: " << error;
<< "could not load " << path << ": " << error;
// Chọn driver theo DỮ LIỆU của kịch bản, không theo một khoá cấu hình riêng: kịch bản khai vật
// cản nghĩa là nó nói về va chạm thật, và chỉ driver nạp recovery_core thật mới trả lời được. Đây
// cũng là lý do MoveBase2ScenarioDriver báo lỗi setup khi thấy `obstacles` thay vì chạy lặng lẽ.
ScenarioRunner runner;
const ScenarioReport report = runner.run(scenario, driver);
ScenarioReport report;
// KHÔNG gọi setup() ở đây: `ScenarioRunner::run` đã tự gọi. Gọi hai lần từng làm driver nạp
// plugin THẬT dựng registry hai lượt, và lượt cũ giữ con trỏ tới cầu nối vừa bị huỷ — use after
// free, biểu hiện ra ngoài chỉ là một dòng "không lấy được pose" trông như lỗi TF.
(void)error;
if (scenario.obstacles.empty())
{
MoveBase2ScenarioDriver driver;
report = runner.run(scenario, driver);
}
else
{
RecoveryScenarioDriver driver;
report = runner.run(scenario, driver);
EXPECT_GT(driver.startRejections(), 0u)
<< scenario.name
<< ": no behavior REFUSED to start — a test case with obstacles that never reaches a "
"safety branch is checking something other than what it describes";
}
EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report);
}
@@ -102,14 +126,22 @@ INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenari
TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty)
{
const std::vector<std::string> files = scenarioFiles();
EXPECT_FALSE(files.empty()) << "không tìm thấy kịch bản nào trong " << scenarioDir()
<< " — suite sẽ xanh mà không kiểm gì cả";
EXPECT_FALSE(files.empty()) << "no scenario found in " << scenarioDir()
<< " — the suite would go green without checking anything";
}
} // namespace
int main(int argc, char** argv)
{
// Kịch bản có vật cản nạp plugin recovery THẬT qua Boost.DLL; ctest không mang theo biến môi
// trường của shell nên binary phải tự trỏ, giống recovery_runner_test.
#ifdef MOVE_BASE2_TEST_CONFIG_DIR
setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0);
#endif
#ifdef MOVE_BASE2_TEST_LIBRARY_DIR
setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0);
#endif
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

42
test/navigation_server_test.cpp
View File

@@ -74,8 +74,6 @@ ControlLoopConfig baseConfig()
config.position.global_planner_name = "FakeGlobalPlanner";
config.position.local_planner_name = "FakeLocalPlanner";
config.position.default_xy_tolerance = 0.15; // [m]
config.position.default_yaw_tolerance = 0.10; // [rad]
config.docking = config.position;
config.go_straight = config.position;
@@ -217,7 +215,8 @@ TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity)
ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling);
const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist();
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist trả vận tốc đo được thay vì lệnh đã phát";
EXPECT_NEAR(twist.velocity.x, 0.3, 1e-9) << "getTwist returned the measured velocity instead of "
"the command that was published";
EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9);
}
@@ -266,7 +265,8 @@ TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel)
fixture.spin(3);
EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero())
<< "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop";
<< "no request has ever arrived yet the stamp is fresh — the host would publish 0 over "
"teleop";
}
TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
@@ -288,14 +288,16 @@ TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes)
const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(1);
EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace)
<< "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish";
<< "the stamp freezes as soon as the leg ends — the final stop command would never be "
"published";
// Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên.
fixture.spin(12);
const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec();
fixture.spin(3);
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9)
<< "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel";
<< "the grace period is over yet the stamp is still fresh — teleop would never get /cmd_vel "
"back";
}
TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
@@ -311,7 +313,8 @@ TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning)
fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.0));
EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_after_first, 1e-9)
<< "dấu thời gian tự tươi lại dù control loop không chạy — host sẽ tưởng lệnh còn hiệu lực";
<< "the timestamp refreshes itself although the control loop is not running — the host would "
"think the command is still valid";
}
TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame)
@@ -355,7 +358,7 @@ TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached)
fixture.server_.addPointCloud2("/camera/depth/points_proc", robot_sensor_msgs::PointCloud2());
EXPECT_EQ(static_layer->count(), 1U);
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 phải cùng tới VoxelLayer";
EXPECT_EQ(local_voxel->count(), 2U) << "laser + pointcloud2 must both reach the VoxelLayer";
EXPECT_EQ(local_voxel->records()[0].topic, "/b_scan");
EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc");
}
@@ -388,7 +391,8 @@ TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed)
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
ASSERT_EQ(static_layer->count(), 1U) << "static map nhận trước khi gắn costmap không được phát lại";
ASSERT_EQ(static_layer->count(), 1U) << "a static map received before a costmap was attached "
"must not be replayed";
EXPECT_EQ(static_layer->records()[0].topic, "/map");
}
@@ -421,7 +425,7 @@ TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTh
SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map");
fixture.attachCostmaps();
EXPECT_EQ(static_layer->count(), 1U) << "cùng một map bị phát lại hai lần";
EXPECT_EQ(static_layer->count(), 1U) << "the same map was replayed twice";
}
TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach)
@@ -475,11 +479,11 @@ TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap)
{
if (std::isnan(stored[i]))
{
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i;
EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "mismatch at ray " << i;
}
else
{
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i;
EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "mismatch at ray " << i;
}
}
}
@@ -625,7 +629,7 @@ TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately)
fixture.server_.pause();
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling)
<< "pause() đi thẳng vào lõi từ thread host";
<< "pause() goes straight into the core from the host thread";
fixture.spin(1);
EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused);
@@ -686,7 +690,7 @@ TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle)
fixture.server_.cancel();
fixture.spin(1);
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel";
EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause won over cancel";
}
TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce)
@@ -738,7 +742,8 @@ TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce)
}
fixture.server_.stopControlThread();
EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread";
EXPECT_TRUE(left_idle) << "the goal was accepted but no cycle ran — the control thread is "
"missing";
}
TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured)
@@ -763,7 +768,7 @@ TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent)
fixture.configure();
ASSERT_TRUE(fixture.server_.startControlThread(100.0));
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai";
EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "started a second thread";
fixture.server_.stopControlThread();
fixture.server_.stopControlThread(); // không được treo hay sập
@@ -797,7 +802,7 @@ TEST(NavigationServerPlannerData, GettersDoNotShareMutableState)
a.plan.poses.clear();
EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty())
<< "hai lần gọi trả về cùng một vùng nhớ";
<< "two calls returned the same memory";
EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); });
}
@@ -851,7 +856,8 @@ TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock)
fixture.spin(1);
EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9)
<< "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua";
<< "the plan carries a timestamp from another clock than the control loop — the host would "
"treat it as stale and drop it";
}
int main(int argc, char** argv)

14
test/planner_runner_test.cpp
View File

@@ -147,7 +147,8 @@ TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded)
EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerMissingLibrary", error));
EXPECT_FALSE(error.empty());
EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình";
EXPECT_FALSE(runner.configured()) << "configure failed but the object still reports itself as "
"configured";
}
// ================================================================================================
@@ -175,7 +176,8 @@ TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries)
ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerOk"));
EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk");
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "quay lại planner cũ mà vẫn nạp lại thư viện";
EXPECT_EQ(fixture.runner_.loadedCount(), 2U) << "switched back to the previous planner yet "
"reloaded the library";
}
TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive)
@@ -210,7 +212,7 @@ TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached)
EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerInitFails"));
EXPECT_EQ(fixture.runner_.loadedCount(), 1U)
<< "instance hỏng bị cache lại — mọi lần thử sau sẽ nhận lại đúng cái hỏng đó";
<< "a broken instance was cached — every later attempt would get that same broken one back";
}
TEST(PlannerRunner, RefusesEmptyPlannerName)
@@ -349,7 +351,8 @@ TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused)
move_base2::PlanResult result;
ASSERT_TRUE(fixture.runner_.pollPlan(result));
EXPECT_TRUE(result.tag == 1U || (!refused && result.tag == 2U));
EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "còn kết quả thứ hai trong hộp thư";
EXPECT_FALSE(fixture.runner_.pollPlan(result)) << "a second result is still sitting in the "
"mailbox";
}
TEST(PlannerRunner, CancelledPlanProducesNoResult)
@@ -367,7 +370,8 @@ TEST(PlannerRunner, CancelledPlanProducesNoResult)
move_base2::PlanResult result;
EXPECT_FALSE(fixture.runner_.pollPlan(result))
<< "lượt đã huỷ vẫn trả kết quả — bên gọi sẽ bám theo plan tới goal không còn ai yêu cầu";
<< "a cancelled attempt still returned a result — the caller would follow a plan to a goal "
"nobody asks for anymore";
}
TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith)

2
test/plugins/test_global_planner.cpp
View File

@@ -65,7 +65,7 @@ public:
if (behavior_ == Behavior::kThrow)
{
throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception");
throw std::runtime_error("TestGlobalPlanner was asked to throw an exception");
}
plan.clear();

42
test/plugins/test_local_planner.cpp
View File

@@ -19,6 +19,7 @@
* Author: DuongTD
*********************************************************************/
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstddef>
#include <limits>
@@ -55,19 +56,28 @@ public:
kNoCommand, ///< computeVelocityCommands trả false.
kNaN, ///< Sinh lệnh chứa NaN — phải bị chặn tại biên.
kThrow, ///< Ném exception khi tính lệnh.
kRefusesLimits ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
kRefusesLimits, ///< setTwistLinear/Angular trả false (planner không hỗ trợ đặt trần).
kMarkerProbe, ///< Đọc `maker_name` MỘT lần lúc initialize, mã hoá vào lệnh — mô phỏng
///< getMaker() của docking planner để test đường re-init khi đổi marker.
kFootprintProbe ///< Mỗi initialize có generation mới; cần reapply goal/plan mới sinh lệnh.
};
explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior)
{
}
void initialize(robot::NodeHandle& /*parent*/, const std::string& name,
void initialize(robot::NodeHandle& parent, const std::string& name,
std::shared_ptr<tf3::BufferCore> /*tf*/,
robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override
{
// Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file.
name_ = name;
// Như PNKXDockingLocalPlanner::getMaker(): đọc đúng MỘT lần, không bao giờ đọc lại.
parent.param("maker_name", marker_at_init_, std::string(""));
if (behavior_ == Behavior::kFootprintProbe)
{
footprint_generation_ = ++footprint_probe_generation_;
}
}
void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
@@ -99,13 +109,22 @@ public:
switch (behavior_)
{
case Behavior::kThrow:
throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception");
throw std::runtime_error("TestLocalPlanner was asked to throw an exception");
case Behavior::kNoCommand:
// Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception.
throw std::runtime_error("TestLocalPlanner: không sinh được lệnh");
throw std::runtime_error("TestLocalPlanner: could not produce a command");
case Behavior::kNaN:
cmd.velocity.x = std::numeric_limits<double>::quiet_NaN();
return cmd;
case Behavior::kMarkerProbe:
// Mã hoá marker đọc được lúc initialize vào lệnh — bảng cố định, test đối chiếu.
cmd.velocity.x = marker_at_init_ == "dock_a" ? 0.11 : marker_at_init_ == "dock_b" ? 0.22 : 0.0;
return cmd;
case Behavior::kFootprintProbe:
// Nếu refresh chỉ dựng instance mà quên setGoalPose/setPlan lại, probe trả 0 thay vì lệnh
// mang generation mới. Như vậy test kiểm đồng thời cache footprint và khôi phục chặng.
cmd.velocity.x = (saw_goal_ && plan_size_ != 0) ? 0.01 * footprint_generation_ : 0.0;
return cmd;
case Behavior::kOk:
case Behavior::kRefusesLimits:
break;
@@ -181,6 +200,9 @@ public:
private:
Behavior behavior_;
std::string name_;
std::string marker_at_init_; ///< `maker_name` tại thời điểm initialize — không bao giờ đọc lại.
inline static std::atomic<unsigned int> footprint_probe_generation_{ 0 };
unsigned int footprint_generation_ = 0;
std::size_t plan_size_ = 0;
bool saw_goal_ = false;
double limit_forward_ = 0.0; ///< [m/s]
@@ -220,6 +242,16 @@ robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
}
robot_nav_core2::LocalPlanner::Ptr createMarkerProbe()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kMarkerProbe);
}
robot_nav_core2::LocalPlanner::Ptr createFootprintProbe()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kFootprintProbe);
}
} // namespace testing
} // namespace move_base2
@@ -229,3 +261,5 @@ BOOST_DLL_ALIAS(move_base2::testing::createNoCommand, TestControllerNoCommand)
BOOST_DLL_ALIAS(move_base2::testing::createNaN, TestControllerNaN)
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestControllerThrowing)
BOOST_DLL_ALIAS(move_base2::testing::createRefusingLimits, TestControllerRefusesLimits)
BOOST_DLL_ALIAS(move_base2::testing::createMarkerProbe, TestControllerMarkerProbe)
BOOST_DLL_ALIAS(move_base2::testing::createFootprintProbe, TestControllerFootprintProbe)

44
test/recovery_runner_test.cpp
View File

@@ -44,7 +44,12 @@ struct Rig
{
runner.setNamespace(ns);
robot::NodeHandle nh;
return runner.configure(nh);
if (!runner.configure(nh))
{
return false;
}
std::string error;
return runner.configureRoutes(nh, error);
}
FakeClockPort clock{1000.0};
@@ -62,6 +67,40 @@ TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder)
EXPECT_EQ(rig.runner.behaviorName(1), "wait_long");
}
TEST(RecoveryRunner, ResolvesPerTriggerRoutesByBehaviorName)
{
Rig rig;
ASSERT_TRUE(rig.load("recovery"));
const move_base2::RecoveryRoutes& routes = rig.runner.routes();
ASSERT_EQ(routes.planning_failed.size(), 1u);
EXPECT_EQ(routes.planning_failed[0], 0u); // wait_short
ASSERT_EQ(routes.controlling_failed.size(), 2u);
EXPECT_EQ(routes.controlling_failed[0], 1u); // wait_long
EXPECT_EQ(routes.controlling_failed[1], 0u); // wait_short
ASSERT_EQ(routes.oscillation.size(), 1u);
EXPECT_EQ(routes.oscillation[0], 1u); // wait_long
}
TEST(RecoveryRunner, SkipsFuturePluginFromRoutesWithoutCreatingInvalidIndexes)
{
Rig rig;
rig.runner.setNamespace("recovery_missing_detour");
robot::NodeHandle nh;
// Registry báo false vì plugin chưa có, nhưng wait vẫn được nạp và phải dùng được.
EXPECT_FALSE(rig.runner.configure(nh));
ASSERT_EQ(rig.runner.behaviorCount(), 1u);
std::string error;
ASSERT_TRUE(rig.runner.configureRoutes(nh, error)) << error;
const move_base2::RecoveryRoutes& routes = rig.runner.routes();
ASSERT_EQ(routes.controlling_failed.size(), 1u);
EXPECT_EQ(routes.controlling_failed[0], 0u);
ASSERT_EQ(routes.oscillation.size(), 1u);
EXPECT_EQ(routes.oscillation[0], 0u);
}
TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors)
{
Rig rig;
@@ -210,7 +249,8 @@ TEST(RecoveryRunner, ConfigureRequiresClockAndPose)
runner.setNamespace("recovery");
robot::NodeHandle nh;
EXPECT_FALSE(runner.configure(nh)) << "thiếu ClockPort/PosePort phải hỏng ngay, không phải lúc tick";
EXPECT_FALSE(runner.configure(nh)) << "a missing ClockPort/PosePort must fail right away, not at "
"tick time";
}
TEST(RecoveryRunner, ConfigureTwiceRejected)

555
test/recovery_scenario_driver.h Normal file
View File

@@ -0,0 +1,555 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — ScenarioDriver thứ hai: recovery_core THẬT trên một thế giới giả có vật cản.
*
* Vì sao cần driver thứ hai. @ref MoveBase2ScenarioDriver mô tả **hành vi của các cổng**: recovery
* của nó là một script, nên nó kiểm được "lõi phản ứng đúng khi recovery báo hỏng" nhưng không bao
* giờ kiểm được "recovery có tự phát hiện ra vật cản sau lưng hay không" — câu hỏi đó chỉ có nghĩa
* khi plugin thật chạy trên một lưới thật có vật cản thật.
*
* Ba thứ driver này có mà driver kia không có:
* 1. **Plugin thật**, nạp qua đúng đường Boost.DLL mà runtime đi.
* 2. **Lưới và va chạm thật** — `FakeCostmap` + `FakeCollisionChecker` của harness, vật cản lấy
* thẳng từ `Scenario::obstacles`.
* 3. **Robot thật sự di chuyển**: lệnh vận tốc phát ra được tích phân vào pose mỗi cycle. Không có
* phần này thì `BackUpRecovery` không bao giờ tiến tới vật cản, và ca test "lùi vào vật cản" chỉ
* là một cái tên.
*
* Author: DuongTD
*********************************************************************/
#ifndef MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
#define MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_
#include <cmath>
#include <cstddef>
#include <memory>
#include <string>
#include <vector>
#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>
#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>
#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_registry.h>
#include <move_base2/control_loop.h>
#include "fake_ports.h"
namespace move_base2
{
namespace testing
{
/// @brief Nối `nav_test_harness::FakePoseProvider` vào cổng pose của recovery_core.
class HarnessPoseProvider final : public recovery_core::PoseProvider
{
public:
explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
{
}
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
{
return fake_ != nullptr && fake_->getRobotPose(pose);
}
private:
nav_test_harness::FakePoseProvider* fake_ = nullptr; ///< non-owning
};
/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng va chạm của recovery_core.
class HarnessCollisionChecker final : public recovery_core::CollisionChecker
{
public:
explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
{
}
double footprintCost(double x, double y, double theta) const override
{
// Không có checker thì coi như không đặt được — giả định an toàn, không phải 0.
return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
}
private:
nav_test_harness::FakeCollisionChecker* fake_ = nullptr; ///< non-owning
};
/**
* @class RegistryRecoveryPort
* @brief Hiện thực @ref RecoveryPort trên một `recovery_core::RecoveryRegistry` đã nạp sẵn.
*
* Là bản rút gọn của @ref RecoveryRunner: cùng ánh xạ enum, cùng ngữ nghĩa tick, nhưng lấy context
* từ ngoài thay vì dựng từ `Costmap2DROBOT`. Nhờ vậy plugin thật chạy được trên lưới giả.
*
* @note Cố ý **không** dùng lại `RecoveryRunner`: cổng pose/va chạm của nó được dựng từ costmap thật
* và không bơm được từ ngoài. Thêm một seam chỉ-dành-cho-test vào lớp runtime để test dễ hơn
* là đổi runtime vì test — hướng phụ thuộc sai.
*/
class RegistryRecoveryPort final : public RecoveryPort
{
public:
explicit RegistryRecoveryPort(recovery_core::RecoveryRegistry* registry, ClockPort* clock)
: registry_(registry), clock_(clock)
{
}
bool configure(robot::NodeHandle& /*nh*/) override
{
// Registry đã được nạp bởi driver trước khi control loop chạy.
return registry_ != nullptr && registry_->size() > 0;
}
std::size_t behaviorCount() const override
{
return registry_ == nullptr ? 0 : registry_->size();
}
RecoveryOutputKind outputKind(std::size_t index) const override
{
recovery_core::RecoveryBehavior* behavior = behaviorAt(index);
if (behavior == nullptr)
{
return RecoveryOutputKind::kNone;
}
switch (behavior->outputKind())
{
case recovery_core::RecoveryOutputType::kVelocity:
return RecoveryOutputKind::kVelocity;
case recovery_core::RecoveryOutputType::kPath:
return RecoveryOutputKind::kPath;
case recovery_core::RecoveryOutputType::kNone:
break;
}
return RecoveryOutputKind::kNone;
}
bool start(std::size_t index, RecoveryTrigger trigger) override
{
active_ = behaviorAt(index);
if (active_ == nullptr || clock_ == nullptr)
{
return false;
}
recovery_core::RecoveryGoal goal;
switch (trigger)
{
case RecoveryTrigger::kPlanningFailed:
goal.trigger = recovery_core::RecoveryTrigger::kPlanningFailed;
break;
case RecoveryTrigger::kControllingFailed:
goal.trigger = recovery_core::RecoveryTrigger::kControllingFailed;
break;
case RecoveryTrigger::kOscillation:
goal.trigger = recovery_core::RecoveryTrigger::kOscillation;
break;
}
if (!active_->start(goal, clock_->now()))
{
// Đây chính là đường mà ca "vật cản sau lưng" đi qua: BackUpRecovery quét trước quãng lùi và
// TỪ CHỐI khởi động nếu đã có va chạm — robot không được nhúc nhích lấy một cycle.
++start_rejections_;
active_ = nullptr;
return false;
}
return true;
}
RecoveryTick update() override
{
RecoveryTick tick;
if (active_ == nullptr || clock_ == nullptr)
{
tick.status = RecoveryTick::Status::kFailed;
return tick;
}
const recovery_core::RecoveryResult result = active_->update(clock_->now());
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:
case recovery_core::RecoveryStatus::kCancelled:
case recovery_core::RecoveryStatus::kFailed:
tick.status = RecoveryTick::Status::kFailed;
break;
}
if (const robot_geometry_msgs::Twist* velocity = result.velocity())
{
tick.has_velocity = true;
tick.cmd = *velocity;
}
tick.message = result.message;
return tick;
}
void cancel() override
{
if (active_ != nullptr)
{
active_->cancel();
}
}
std::string behaviorName(std::size_t index) const override
{
return registry_ != nullptr && index < registry_->size() ? registry_->nameAt(index)
: std::string();
}
/// @brief Số lần behavior từ chối khởi động — bằng chứng ca test đi đúng nhánh nó nói là đang kiểm.
std::size_t startRejections() const
{
return start_rejections_;
}
private:
recovery_core::RecoveryBehavior* behaviorAt(std::size_t index) const
{
return registry_ == nullptr ? nullptr : registry_->at(index);
}
recovery_core::RecoveryRegistry* registry_ = nullptr; ///< non-owning
ClockPort* clock_ = nullptr; ///< non-owning
recovery_core::RecoveryBehavior* active_ = nullptr; ///< non-owning
std::size_t start_rejections_ = 0;
};
/**
* @class RecoveryScenarioDriver
* @brief Chạy kịch bản qua @ref ControlLoop với recovery_core THẬT trên lưới giả có vật cản.
*
* Planner và controller vẫn là cổng giả theo script: ca test ở đây nói về **recovery**, và một
* planner thật sẽ làm kết quả phụ thuộc vào chất lượng đường đi thay vì vào thứ đang được kiểm.
*/
class RecoveryScenarioDriver final : public nav_test_harness::ScenarioDriver
{
public:
bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
{
scenario_ = scenario;
// Gọi setup() lần thứ hai phải dựng lại từ đầu, và THỨ TỰ tháo là bắt buộc: behavior trong
// registry giữ con trỏ tới pose/collision bridge qua RecoveryContext, nên registry phải chết
// TRƯỚC chúng. Tháo ngược lại là use-after-free, và triệu chứng của nó ("không lấy được pose")
// trông y hệt một lỗi TF bình thường.
recovery_port_.reset();
registry_.reset();
collision_bridge_.reset();
pose_bridge_.reset();
checker_.reset();
costmap_.reset();
if (scenario.obstacles.empty())
{
// Không phải lỗi chết người, nhưng nói ra: driver này tồn tại vì vật cản. Kịch bản không có
// vật cản nào chạy ở đây là đang trả giá dựng plugin thật mà không kiểm thêm được gì.
error = "RecoveryScenarioDriver is for scenarios WITH obstacles; use MoveBase2ScenarioDriver";
return false;
}
// --- Thế giới: lưới, vật cản, pose ban đầu --------------------------------------------------
costmap_.reset(new nav_test_harness::FakeCostmap(nav_test_harness::FakeCostmap::centered(
kWorldSpan, kResolution)));
for (const nav_test_harness::ScenarioObstacle& obstacle : scenario.obstacles)
{
if (costmap_->setLethalCircle(obstacle.x, obstacle.y, obstacle.radius) == 0) // [m]
{
// Vật cản nằm ngoài lưới = kịch bản dựng sai. Im lặng ở đây nghĩa là ca test chạy trên một
// thế giới không có vật cản nào và vẫn xanh.
error = "the obstacle lies outside the fake grid — re-check the coordinates in the "
"scenario";
return false;
}
}
checker_.reset(new nav_test_harness::FakeCollisionChecker(
costmap_.get(),
nav_test_harness::FakeCollisionChecker::rectangleFootprint(kFootprintLength,
kFootprintWidth)));
pose_source_.setPose(scenario.initial_pose.x, scenario.initial_pose.y,
scenario.initial_pose.theta);
pose_bridge_.reset(new HarnessPoseProvider(&pose_source_));
collision_bridge_.reset(new HarnessCollisionChecker(checker_.get()));
recovery_core::RecoveryContext ctx;
ctx.pose = pose_bridge_.get();
ctx.collision = collision_bridge_.get();
// --- Recovery THẬT --------------------------------------------------------------------------
registry_.reset(new recovery_core::RecoveryRegistry());
robot::NodeHandle nh;
if (!registry_->loadFromConfig(nh, kRecoveryNamespace, ctx))
{
error = "could not load the real recovery behaviors from namespace '" +
std::string(kRecoveryNamespace) + "' — check library_path in the test config";
return false;
}
if (registry_->size() == 0)
{
error = "the recovery namespace is empty — the scenario would run without checking anything";
return false;
}
recovery_port_.reset(new RegistryRecoveryPort(registry_.get(), &clock_));
// --- Cổng giả cho phần còn lại --------------------------------------------------------------
std::vector<PlannerScript> planner_script;
for (const std::string& item : scenario.planner_script)
{
if (item == "ok")
{
planner_script.push_back(PlannerScript::kOk);
}
else if (item == "fail")
{
planner_script.push_back(PlannerScript::kFail);
}
else if (item == "empty")
{
planner_script.push_back(PlannerScript::kEmpty);
}
else
{
error = "unknown planner_script: '" + item + "'";
return false;
}
}
planner_.setScript(planner_script);
std::vector<ControllerScript> controller_script;
for (const std::string& item : scenario.controller_script)
{
if (item == "ok")
{
controller_script.push_back(ControllerScript::kOk);
}
else if (item == "fail")
{
controller_script.push_back(ControllerScript::kFail);
}
else if (item == "goal_reached")
{
controller_script.push_back(ControllerScript::kGoalReached);
}
else
{
error = "controller_script is not supported by this driver: '" + item + "'";
return false;
}
}
controller_.setScript(controller_script);
if (!scenario.recovery_script.empty())
{
error = "recovery_script cannot be used here: recovery is a REAL plugin, its result is "
"decided by collisions and geometry, not assigned by the scenario";
return false;
}
pose_port_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);
ControlLoopConfig config;
config.nominal_control_period = scenario.control_period; // [s]
config.state_machine.planner_patience = 0.5; // [s]
config.state_machine.controller_patience = 0.5; // [s]
config.state_machine.oscillation_timeout = 0.0; // tắt
config.state_machine.oscillation_distance = 0.5; // [m]
config.state_machine.max_planning_retries = -1;
config.state_machine.recovery_behavior_count = registry_->size();
config.state_machine.recovery_enabled = true;
config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
config.velocity.min_vel_x = -config.velocity.max_vel_x;
config.velocity.max_vel_theta =
scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
config.velocity.max_accel_x = 100.0; // [m/s^2] lớn: ca test kiểm va chạm, không kiểm ramp
config.velocity.max_accel_theta = 100.0; // [rad/s^2]
config.position.global_planner_name = "ScenarioGlobalPlanner";
config.position.local_planner_name = "ScenarioLocalPlanner";
config.docking = config.position;
config.go_straight = config.position;
config.rotate = config.position;
deps_.clock = &clock_;
deps_.pose = &pose_port_;
deps_.planner = &planner_;
deps_.controller = &controller_;
deps_.recovery = recovery_port_.get();
deps_.mission = &mission_;
deps_.action = &action_;
if (!loop_.configure(config, deps_, error))
{
return false;
}
NavigationRequest request;
request.profile = MotionProfile::kPosition;
request.goal.header.frame_id = "map";
request.goal.pose.position.x = scenario.goal.x; // [m]
request.goal.pose.position.y = scenario.goal.y; // [m]
request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);
if (!loop_.submit(request, error))
{
return false;
}
cycle_ = 0;
started_ = false;
return true;
}
bool step(nav_test_harness::ScenarioStep& step) override
{
if (!started_)
{
started_ = true;
step.cycle = 0;
step.state = toString(loop_.state());
step.linear_x = 0.0;
step.angular_z = 0.0;
return true;
}
applyEventsFor(cycle_);
const bool running = loop_.step();
const robot_geometry_msgs::Twist& command = loop_.lastCommand();
step.cycle = cycle_;
step.state = toString(loop_.state());
step.linear_x = command.linear.x; // [m/s]
step.angular_z = command.angular.z; // [rad/s]
// Robot thật sự đi theo lệnh vừa phát. Đây là điểm khác biệt của driver này: không có tích phân
// chuyển động thì pose đứng yên, `BackUpRecovery` không bao giờ đo được quãng đã lùi, và ca test
// "lùi vào vật cản" sẽ kết thúc vì hết giờ chứ không vì va chạm — xanh vì lý do sai.
integrateMotion(command, scenario_.control_period);
clock_.advance(scenario_.control_period);
++cycle_;
return running;
}
std::string outcome() const override
{
const char* text = loop_.lastOutcome();
return text != nullptr ? std::string(text) : std::string();
}
/// @brief Số lần behavior từ chối khởi động — dùng để khẳng định ca test đi đúng nhánh.
std::size_t startRejections() const
{
return recovery_port_ == nullptr ? 0 : recovery_port_->startRejections();
}
private:
/// [m] Cạnh của lưới giả, đủ rộng để quãng lùi và footprint không chạm biên.
static constexpr double kWorldSpan = 8.0;
static constexpr double kResolution = 0.05; ///< [m/ô] khớp costmap thật của workspace
static constexpr double kFootprintLength = 0.6; ///< [m]
static constexpr double kFootprintWidth = 0.4; ///< [m]
static constexpr const char* kRecoveryNamespace = "recovery_scenario";
void integrateMotion(const robot_geometry_msgs::Twist& command, double dt)
{
const double yaw = pose_source_.rawPose().theta; // [rad]
pose_source_.moveBy(command.linear.x * std::cos(yaw) * dt,
command.linear.x * std::sin(yaw) * dt, command.angular.z * dt);
// Hai nguồn pose phải đi cùng nhau: `pose_source_` là thứ recovery_core nhìn thấy, `pose_port_`
// là thứ lõi nhìn thấy. Lệch nhau thì chống quẩn và recovery nói về hai robot khác nhau.
const auto& pose = pose_source_.rawPose();
pose_port_.setPosition(pose.x, pose.y);
}
void applyEventsFor(std::size_t cycle)
{
for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
{
if (event.cycle != cycle)
{
continue;
}
if (event.action == "cancel")
{
loop_.requestCancel();
}
else if (event.action == "pause")
{
loop_.requestPause();
}
else if (event.action == "resume")
{
loop_.requestResume();
}
else if (event.action == "lose_pose")
{
pose_port_.setAvailable(false);
}
else if (event.action == "restore_pose")
{
pose_port_.setAvailable(true);
}
else if (event.action == "sensors_stale")
{
costmap_status_.setCurrent(false);
}
else if (event.action == "sensors_ok")
{
costmap_status_.setCurrent(true);
}
}
}
nav_test_harness::Scenario scenario_;
std::unique_ptr<nav_test_harness::FakeCostmap> costmap_;
std::unique_ptr<nav_test_harness::FakeCollisionChecker> checker_;
nav_test_harness::FakePoseProvider pose_source_;
std::unique_ptr<HarnessPoseProvider> pose_bridge_;
std::unique_ptr<HarnessCollisionChecker> collision_bridge_;
// Khai SAU các cầu nối: thành viên bị huỷ theo thứ tự ngược, nên registry (giữ con trỏ tới chúng)
// chết trước — cùng lý do với thứ tự tháo trong setup().
std::unique_ptr<recovery_core::RecoveryRegistry> registry_;
std::unique_ptr<RegistryRecoveryPort> recovery_port_;
ControlLoop loop_;
ControlLoopDeps deps_;
FakeClockPort clock_;
FakePosePort pose_port_;
FakePlannerPort planner_;
FakeControllerPort controller_;
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
std::size_t cycle_ = 0;
bool started_ = false;
};
} // namespace testing
} // namespace move_base2
#endif // MOVE_BASE2_TEST_RECOVERY_SCENARIO_DRIVER_H_

184
test/runtime_stats_test.cpp Normal file
View File

@@ -0,0 +1,184 @@
/**
* @file runtime_stats_test.cpp
* @brief Kiểm @ref move_base2::RuntimeStats.
*
* Ba tính chất được khoá lại ở đây, đều là thứ mà một lỗi ở chúng sẽ làm bảng thống kê nói dối chứ
* không làm chương trình chết:
*
* 1. **Tắt là tắt hẳn** — `period <= 0` thì không đăng ký được đoạn nào, không đo, không in.
* Telemetry bật ngoài ý muốn trên robot thật nghĩa là log chen vào vòng điều khiển.
* 2. **Cửa sổ được reset sau mỗi lần in** — số liệu là của cửa sổ vừa qua, không phải tích luỹ từ
* lúc khởi động. Cộng dồn mãi thì mọi đỉnh tức thời sẽ bị pha loãng và không bao giờ thấy lại.
* 3. **Thread tự tạo được gán nhãn qua cửa sổ chụp** — đây là cơ chế duy nhất gọi tên được thread
* của costmap mà không phải sửa gói costmap.
*/
#include <move_base2/io/runtime_stats.h>
#include <atomic>
#include <chrono>
#include <thread>
#include <gtest/gtest.h>
namespace
{
using move_base2::RuntimeStats;
using move_base2::ScopedSection;
/// Đốt CPU thật trong khoảng @p ms — sleep không làm tăng bộ đếm CPU của thread.
void burnCpu(int ms)
{
const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(ms);
volatile double sink = 0.0;
while (std::chrono::steady_clock::now() < deadline)
{
for (int i = 0; i < 1000; ++i)
{
sink += static_cast<double>(i) * 1.000001;
}
}
(void)sink;
}
TEST(RuntimeStatsTest, DisabledMeansNoWork)
{
RuntimeStats stats(0.0);
EXPECT_FALSE(stats.enabled());
EXPECT_EQ(stats.section("anything"), RuntimeStats::kInvalidSection);
EXPECT_FALSE(stats.tick()) << "period = 0 yet it still printed to the terminal";
// Không được crash dù chỉ số không hợp lệ.
stats.record(RuntimeStats::kInvalidSection, 1000);
stats.registerCurrentThread("must not be recorded");
const std::string table = stats.render();
EXPECT_EQ(table.find("anything"), std::string::npos);
}
TEST(RuntimeStatsTest, NegativePeriodDisables)
{
RuntimeStats stats(-1.0);
EXPECT_FALSE(stats.enabled());
}
TEST(RuntimeStatsTest, SectionAccumulatesAndResetsPerWindow)
{
RuntimeStats stats(3600.0); // chu kỳ dài: chỉ render() thủ công mới đóng cửa sổ
ASSERT_TRUE(stats.enabled());
const RuntimeStats::SectionId id = stats.section("test.section");
ASSERT_NE(id, RuntimeStats::kInvalidSection);
stats.record(id, 1'000'000); // 1 ms
stats.record(id, 3'000'000); // 3 ms
const std::string first = stats.render();
EXPECT_NE(first.find("test.section"), std::string::npos);
EXPECT_NE(first.find("2.00"), std::string::npos) << "average must be 2.00 ms:\n" << first;
EXPECT_NE(first.find("3.00"), std::string::npos) << "peak must be 3.00 ms:\n" << first;
// Cửa sổ mới: đoạn vẫn còn trong bảng nhưng số liệu về 0.
const std::string second = stats.render();
EXPECT_NE(second.find("test.section"), std::string::npos);
EXPECT_NE(second.find("0.00"), std::string::npos) << "a new window must reset:\n" << second;
}
TEST(RuntimeStatsTest, SameNameReturnsSameId)
{
RuntimeStats stats(3600.0);
EXPECT_EQ(stats.section("loop"), stats.section("loop"));
}
TEST(RuntimeStatsTest, TickOnlyPrintsWhenPeriodElapsed)
{
RuntimeStats stats(3600.0);
EXPECT_FALSE(stats.tick()) << "printed before the period elapsed";
RuntimeStats fast(0.001); // [s]
std::this_thread::sleep_for(std::chrono::milliseconds(5));
EXPECT_TRUE(fast.tick());
EXPECT_FALSE(fast.tick()) << "the window must be reopened right after printing";
}
TEST(RuntimeStatsTest, RegisteredThreadAppearsWithCpuTime)
{
RuntimeStats stats(3600.0);
std::atomic<bool> registered{ false };
std::atomic<bool> stop{ false };
std::thread worker([&stats, &registered, &stop]() {
stats.registerCurrentThread("test/worker");
registered.store(true);
while (!stop.load())
{
burnCpu(5);
}
});
while (!registered.load())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
burnCpu(60);
const std::string table = stats.render();
stop.store(true);
worker.join();
EXPECT_NE(table.find("test/worker"), std::string::npos) << table;
EXPECT_NE(table.find("(unregistered)"), std::string::npos)
<< "CPU outside the registered threads must show up, otherwise the table hides the culprit:\n"
<< table;
}
TEST(RuntimeStatsTest, ThreadCaptureLabelsThreadsCreatedInsideWindow)
{
RuntimeStats stats(3600.0);
std::atomic<bool> stop{ false };
std::thread created;
stats.beginThreadCapture();
created = std::thread([&stop]() {
while (!stop.load())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
});
// Thread phải thực sự tồn tại trong /proc trước khi đóng cửa sổ chụp.
std::this_thread::sleep_for(std::chrono::milliseconds(20));
stats.endThreadCapture("simulated/costmap");
const std::string table = stats.render();
stop.store(true);
created.join();
#ifdef __linux__
EXPECT_NE(table.find("simulated/costmap"), std::string::npos) << table;
#else
GTEST_SKIP() << "thread capture windows rely on /proc, Linux only";
#endif
}
TEST(RuntimeStatsTest, ScopedSectionToleratesNullStats)
{
// Đây là đường đi bình thường của mọi test dùng cổng giả: runner không được gắn telemetry.
{
ScopedSection timer(nullptr, RuntimeStats::kInvalidSection);
}
RuntimeStats stats(0.0);
{
ScopedSection timer(&stats, stats.section("skipped"));
}
SUCCEED();
}
} // namespace
int main(int argc, char** argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

6
test/sensor_gateway_test.cpp
View File

@@ -233,7 +233,8 @@ TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample)
bench.gateway().pushLaserScan("/b_scan", makeScan());
EXPECT_EQ(trap->count(), 0U) << "layer trùng TÊN topic nhưng sai KIỂU vẫn nhận được dữ liệu";
EXPECT_EQ(trap->count(), 0U) << "a layer matching the topic NAME but with the wrong TYPE still "
"received data";
EXPECT_EQ(voxel->count(), 1U);
}
@@ -310,7 +311,8 @@ TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes)
bench.gateway().pushLaserScan("/b_scan", makeScan());
EXPECT_EQ(exploding->count(), 0U);
EXPECT_EQ(healthy->count(), 1U) << "layer lành bị bỏ qua vì layer trước nó ném exception";
EXPECT_EQ(healthy->count(), 1U) << "a healthy layer was skipped because the layer before it "
"threw an exception";
EXPECT_EQ(bench.gateway().stats().layer_exceptions, 1U);
EXPECT_EQ(bench.gateway().stats().delivered, 1U);
}

2
test/spy_layer.h
View File

@@ -81,7 +81,7 @@ protected:
{
if (explode_)
{
throw std::runtime_error("SpyLayer được yêu cầu ném exception");
throw std::runtime_error("SpyLayer was asked to throw an exception");
}
records_.push_back(Record{ &type, topic });
if (observer_)

115
test/state_machine_test.cpp
View File

@@ -193,7 +193,8 @@ TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors)
config.recovery_enabled = true;
std::string error;
EXPECT_FALSE(config.validate(error)) << "cấu hình này lúc chạy sẽ ABORTED ngay ở lỗi đầu tiên";
EXPECT_FALSE(config.validate(error)) << "this config would go ABORTED at runtime on the very "
"first failure";
}
TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
@@ -206,6 +207,18 @@ TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
EXPECT_TRUE(config.validate(error)) << error;
}
TEST(StateMachineConfig, RejectsResolvedRouteWithAnOutOfRangeBehavior)
{
StateMachineConfig config = baseConfig();
config.recovery_routes.planning_failed = {0};
config.recovery_routes.controlling_failed = {1};
config.recovery_routes.oscillation = {2}; // behavior_count chỉ là 2
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("loaded behavior"), std::string::npos);
}
TEST(StateMachineConfig, DescribeMentionsEveryParameter)
{
const std::string text = baseConfig().describe();
@@ -341,6 +354,27 @@ TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires)
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}
TEST(StateMachinePlanning, UsesPlanningRouteInsteadOfRegistryOrder)
{
StateMachineConfig config = baseConfig();
config.max_planning_retries = 0;
config.planner_patience = 100.0;
config.recovery_routes.planning_failed = {1};
config.recovery_routes.controlling_failed = {0};
config.recovery_routes.oscillation = {0};
Driver driver(config);
StateMachineInput request;
request.has_pending_request = true;
driver.tick(request);
const StateMachineOutput out = driver.tick(plannerFailed());
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_TRUE(out.start_recovery);
EXPECT_EQ(out.recovery_index, 1u);
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}
TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
{
// `planner_patience = 0` vẫn nghĩa là "tắt đồng hồ kiên nhẫn". Nhưng từ khi planner chạy trên
@@ -359,7 +393,7 @@ TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
for (int i = 0; i < 100; ++i)
{
driver.advance(1.0);
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "vòng " << i;
ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "round " << i;
}
}
@@ -475,13 +509,13 @@ TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop)
SUCCEED();
return;
}
ASSERT_EQ(state, NavigationState::kPlanning) << "vòng " << i;
ASSERT_EQ(state, NavigationState::kPlanning) << "round " << i;
driver.advance(0.05);
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "vòng " << i;
ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "round " << i;
}
FAIL() << "controller_patience không bao giờ hết hạn — vòng lặp lập-plan đã làm mới đồng hồ";
FAIL() << "controller_patience never expires — the replanning loop kept refreshing the clock";
}
TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
@@ -504,6 +538,24 @@ TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
}
TEST(StateMachineControlling, UsesOscillationRouteIndependently)
{
StateMachineConfig config = baseConfig();
config.oscillation_timeout = 1.0;
config.oscillation_distance = 0.5;
config.recovery_routes.planning_failed = {0};
config.recovery_routes.controlling_failed = {0};
config.recovery_routes.oscillation = {1};
Driver driver(config);
driver.driveToControlling();
driver.advance(1.1);
const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
EXPECT_EQ(out.recovery_index, 1u);
}
TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
{
StateMachineConfig config = baseConfig();
@@ -519,8 +571,8 @@ TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
input.travelled_since_oscillation_reset = 0.6; // [m] đi đủ xa mỗi lần
const StateMachineOutput out = driver.tick(input);
ASSERT_EQ(out.state, NavigationState::kControlling) << "vòng " << i;
ASSERT_TRUE(out.reset_oscillation_origin) << "vòng " << i;
ASSERT_EQ(out.state, NavigationState::kControlling) << "round " << i;
ASSERT_TRUE(out.reset_oscillation_origin) << "round " << i;
}
}
@@ -597,7 +649,7 @@ TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers)
const StateMachineOutput first = driver.tick(blind);
EXPECT_EQ(first.velocity_source, VelocitySource::kNone)
<< "không biết robot ở đâu thì không nguồn nào được phát vận tốc";
<< "when the robot position is unknown no source may publish velocity";
EXPECT_FALSE(first.run_controller);
// Mất TF kéo dài phải dẫn tới recovery, không được treo im lặng.
@@ -764,7 +816,7 @@ TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning
EXPECT_EQ(paused.state, NavigationState::kPaused);
EXPECT_TRUE(paused.cancel_recovery)
<< "giữ một behavior dở dang qua quãng dừng dài là không an toàn";
<< "keeping a behavior half-finished across a long pause is unsafe";
StateMachineInput resume;
resume.resume_requested = true;
@@ -785,7 +837,8 @@ TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity)
const StateMachineOutput out = driver.tick(blind);
EXPECT_EQ(out.state, NavigationState::kRecovering);
EXPECT_TRUE(out.tick_recovery) << "vẫn tick để behavior tự báo lỗi theo contract của nó";
EXPECT_TRUE(out.tick_recovery) << "still ticked so the behavior reports its own failure per its "
"contract";
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}
@@ -819,7 +872,7 @@ TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume)
for (int i = 0; i < 50; ++i)
{
driver.advance(1.0);
ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "vòng " << i;
ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "round " << i;
ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone);
}
}
@@ -859,7 +912,7 @@ TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce)
for (int i = 0; i < 10; ++i)
{
const StateMachineOutput out = driver.idleTick();
ASSERT_FALSE(out.report_outcome) << "báo lại ở vòng " << i;
ASSERT_FALSE(out.report_outcome) << "reported again at round " << i;
ASSERT_EQ(out.state, NavigationState::kIdle);
}
}
@@ -910,7 +963,7 @@ TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely)
EXPECT_TRUE(accepted.accept_request);
EXPECT_TRUE(accepted.start_action);
EXPECT_EQ(accepted.action_index, 0u);
EXPECT_FALSE(accepted.start_planner) << "không có goal thì không có gì để lập plan";
EXPECT_FALSE(accepted.start_planner) << "with no goal there is nothing to plan";
EXPECT_EQ(accepted.velocity_source, VelocitySource::kNone);
ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
@@ -973,7 +1026,7 @@ TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery)
EXPECT_EQ(out.state, NavigationState::kAborted);
EXPECT_TRUE(out.report_outcome);
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
EXPECT_FALSE(out.start_recovery) << "recovery là công cụ phục hồi navigation, không cứu được action";
EXPECT_FALSE(out.start_recovery) << "recovery repairs navigation, it cannot rescue an action";
}
TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled)
@@ -1005,15 +1058,15 @@ TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling)
pause.pause_requested = true;
const StateMachineOutput paused = driver.tick(pause);
EXPECT_EQ(paused.state, NavigationState::kPaused);
EXPECT_FALSE(paused.cancel_action) << "action không idempotent — tạm dừng không được huỷ nó";
EXPECT_FALSE(paused.cancel_action) << "the action is not idempotent — pausing must not cancel it";
EXPECT_FALSE(paused.tick_action);
StateMachineInput resume;
resume.resume_requested = true;
const StateMachineOutput resumed = driver.tick(resume);
EXPECT_EQ(resumed.state, NavigationState::kExecutingActions);
EXPECT_TRUE(resumed.tick_action) << "resume tick tiếp action dở dang";
EXPECT_FALSE(resumed.start_action) << "không được start lại action đã chạy dở";
EXPECT_TRUE(resumed.tick_action) << "resume keeps ticking the unfinished action";
EXPECT_FALSE(resumed.start_action) << "an action already in progress must not be started again";
}
TEST(StateMachineActions, ActionPatienceIsDisabledByDefault)
@@ -1045,7 +1098,8 @@ TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort)
driver.advance(1.0); // Tổng 1.5 s > action_patience.
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
EXPECT_EQ(out.state, NavigationState::kAborted);
EXPECT_TRUE(out.cancel_action) << "phải bảo port dừng thiết bị an toàn trước khi kết thúc";
EXPECT_TRUE(out.cancel_action) << "the port must be told to stop the device safely before "
"finishing";
EXPECT_TRUE(out.report_outcome);
EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
}
@@ -1071,7 +1125,8 @@ TEST(StateMachineActions, PauseRearmsActionPatienceClock)
driver.advance(0.5); // Mới 0.5 s sau resume, chưa chạm trần.
const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "resume xong không được ABORTED oan";
EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "must not be wrongly ABORTED after "
"resume";
EXPECT_TRUE(out.tick_action);
}
@@ -1086,7 +1141,7 @@ TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero)
const StateMachineOutput out = driver.tick(no_pose);
EXPECT_EQ(out.state, NavigationState::kExecutingActions);
EXPECT_TRUE(out.tick_action) << "thao tác thiết bị tại chỗ không cần định vị";
EXPECT_TRUE(out.tick_action) << "operating a device in place needs no localization";
EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}
@@ -1121,16 +1176,18 @@ TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously)
const StateMachineOutput out = driver.tick(input);
ASSERT_FALSE(out.run_controller && out.tick_recovery)
<< "state " << move_base2::toString(out.state) << ": hai nguồn lệnh cùng chạy";
<< "state " << move_base2::toString(out.state) << ": two command sources running at once";
ASSERT_FALSE(out.run_controller && out.tick_action)
<< "state " << move_base2::toString(out.state) << ": controller chạy cùng action";
<< "state " << move_base2::toString(out.state) << ": controller running together with an "
"action";
ASSERT_FALSE(out.tick_recovery && out.tick_action)
<< "state " << move_base2::toString(out.state) << ": recovery chạy cùng action";
<< "state " << move_base2::toString(out.state) << ": recovery running together with an "
"action";
if (move_base2::mustBeStopped(out.state))
{
ASSERT_EQ(out.velocity_source, VelocitySource::kNone)
<< "state " << move_base2::toString(out.state) << " phải dừng";
<< "state " << move_base2::toString(out.state) << " must be stopped";
}
if (out.velocity_source == VelocitySource::kController)
{
@@ -1192,7 +1249,8 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning)
for (int i = 1; i <= 5; ++i)
{
driver.advance(0.05);
EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning) << "rời PLANNING ở cycle " << i;
EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning)
<< "left PLANNING at cycle " << i;
}
}
@@ -1218,7 +1276,7 @@ TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt)
}
EXPECT_EQ(driver.machine().state(), NavigationState::kPlanning)
<< "kBusy bị tính là lượt lập plan hỏng nên đã cạn max_planning_retries";
<< "kBusy was counted as a failed planning attempt so max_planning_retries ran out";
}
TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
@@ -1234,7 +1292,8 @@ TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
const StateMachineOutput out = driver.tick(busy);
EXPECT_EQ(out.state, NavigationState::kControlling);
EXPECT_FALSE(out.apply_plan) << "đẩy plan xuống controller khi planner chưa có plan nào";
EXPECT_FALSE(out.apply_plan) << "pushed a plan down to the controller while the planner had no "
"plan yet";
}
TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
@@ -1247,7 +1306,7 @@ TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
std::string error;
EXPECT_FALSE(config.validate(error));
EXPECT_NE(error.find("planner treo"), std::string::npos) << error;
EXPECT_NE(error.find("hung planner"), std::string::npos) << error;
}
int main(int argc, char** argv)

14
test/velocity_arbiter_test.cpp
View File

@@ -97,7 +97,7 @@ TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero)
{
VelocityLimits limits = baseLimits();
limits.min_vel_x = 0.0;
EXPECT_NE(limits.describe().find("cấm lùi"), std::string::npos);
EXPECT_NE(limits.describe().find("reversing forbidden"), std::string::npos);
}
TEST(VelocityArbiter, RefusesToEmitBeforeConfigure)
@@ -134,7 +134,7 @@ TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately)
const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(0.5, 0.8), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "lệnh 0 phải tức thì, không giảm tốc dần";
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0) << "a zero command must be immediate, not ramped down";
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0);
EXPECT_TRUE(arbiter.stopped());
}
@@ -161,7 +161,8 @@ TEST(VelocityArbiter, NaNIsBlockedAndCounted)
const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(nan_value, 0.3), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0);
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "một trục hỏng làm hỏng cả lệnh, không sửa từng phần";
EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0) << "one broken axis breaks the whole command, it is not "
"patched per axis";
EXPECT_EQ(arbiter.nonFiniteRejections(), 1u);
}
@@ -193,7 +194,8 @@ TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero)
const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-9.0, 0.0), kDt);
EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x là trần LÙI, không phải cận dưới bằng 0";
EXPECT_DOUBLE_EQ(cmd.linear.x, -0.2) << "min_vel_x is the REVERSE limit, not a lower bound of "
"zero";
}
TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero)
@@ -290,11 +292,11 @@ TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle)
ASSERT_NEAR(controlling.linear.x, 0.4, 1e-9);
const auto handover = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "cycle bàn giao phải là 0";
EXPECT_DOUBLE_EQ(handover.linear.x, 0.0) << "the handover cycle must be 0";
EXPECT_EQ(arbiter.handoverCycles(), 1u);
const auto recovering = arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.15, 0.0), kDt);
EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "chỉ đúng MỘT cycle 0, không nhiều hơn";
EXPECT_NEAR(recovering.linear.x, -0.15, 1e-9) << "exactly ONE zero cycle, no more";
}
TEST(VelocityArbiter, HandoverWorksInBothDirections)

303
test/walking_skeleton_test.cpp
View File

@@ -31,6 +31,7 @@ using move_base2::testing::ControllerScript;
using move_base2::testing::FakeActionPort;
using move_base2::testing::FakeClockPort;
using move_base2::testing::FakeControllerPort;
using move_base2::testing::FakeCostmapStatusPort;
using move_base2::testing::FakeMissionPort;
using move_base2::testing::FakePlannerPort;
using move_base2::testing::FakePosePort;
@@ -65,8 +66,6 @@ ControlLoopConfig baseConfig()
config.position.global_planner_name = "FakeGlobalPlanner";
config.position.local_planner_name = "FakeLocalPlanner";
config.position.default_xy_tolerance = 0.15; // [m]
config.position.default_yaw_tolerance = 0.10; // [rad]
config.docking = config.position;
config.docking.local_planner_name = "FakeDockPlanner";
@@ -128,6 +127,7 @@ public:
deps_.recovery = &recovery_;
deps_.mission = &mission_;
deps_.action = &action_;
deps_.costmap_status = &costmap_status_;
std::string error;
EXPECT_TRUE(loop_.configure(config, deps_, error)) << error;
@@ -175,6 +175,7 @@ public:
FakeRecoveryPort recovery_;
FakeMissionPort mission_;
FakeActionPort action_;
FakeCostmapStatusPort costmap_status_;
ControlLoopDeps deps_;
private:
@@ -218,7 +219,7 @@ TEST(ControlLoop, RefusesToConfigureWithMissingPorts)
std::string error;
EXPECT_FALSE(loop.configure(baseConfig(), deps, error));
EXPECT_NE(error.find("cổng"), std::string::npos);
EXPECT_NE(error.find("port"), std::string::npos);
EXPECT_FALSE(loop.initialized());
}
@@ -286,24 +287,89 @@ TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded)
EXPECT_NE(reason.find("global planner"), std::string::npos);
}
TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances)
TEST(ControlLoop, ProfileSelectsItsOwnLocalPlanner)
{
Fixture fixture;
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
docking.marker = "dock_a";
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
EXPECT_NEAR(fixture.controller_.xyTolerance(), 0.15, 1e-9) << "tolerance 0 -> dùng default profile";
// Marker phải tới port TRƯỚC khi swap — planner đọc maker_name trong initialize().
EXPECT_EQ(fixture.controller_.lastDockingMarker(), "dock_a");
NavigationRequest rotate = makeRequest(1.0);
rotate.profile = MotionProfile::kRotate;
rotate.tolerance.yaw = 0.02; // [rad]
ASSERT_TRUE(fixture.loop_.submit(rotate, reason)) << reason;
EXPECT_EQ(fixture.controller_.activeController(), "FakeRotatePlanner");
EXPECT_NEAR(fixture.controller_.yawTolerance(), 0.02, 1e-9);
}
TEST(ControlLoop, DockingMarkerProfileOverridesBothPlannersAndFallsBackToDefault)
{
ControlLoopConfig config = baseConfig();
config.docking_marker_profiles["trolley"].global_planner_name = "FakeTrolleyGlobalPlanner";
config.docking_marker_profiles["trolley"].local_planner_name = "FakeTrolleyLocalPlanner";
Fixture fixture(config);
NavigationRequest trolley = makeRequest(1.0);
trolley.profile = MotionProfile::kDocking;
trolley.marker = "trolley";
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(trolley, reason)) << reason;
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeTrolleyGlobalPlanner");
EXPECT_EQ(fixture.controller_.activeController(), "FakeTrolleyLocalPlanner");
NavigationRequest unconfigured = makeRequest(1.0);
unconfigured.profile = MotionProfile::kDocking;
unconfigured.marker = "charger";
ASSERT_TRUE(fixture.loop_.submit(unconfigured, reason)) << reason;
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeGlobalPlanner");
EXPECT_EQ(fixture.controller_.activeController(), "FakeDockPlanner");
}
TEST(ControlLoop, RejectsDockingRequestWithoutMarker)
{
Fixture fixture;
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
// marker cố ý bỏ trống — bản cũ cũng chặn tại cửa dockTo.
std::string reason;
EXPECT_FALSE(fixture.loop_.submit(docking, reason));
EXPECT_NE(reason.find("marker"), std::string::npos) << reason;
}
TEST(ControlLoop, AllowsGoalFrameStyleDockingWithoutMarkerWhenConfigured)
{
ControlLoopConfig config = baseConfig();
config.docking_requires_marker = false;
Fixture fixture(config);
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
std::string reason;
EXPECT_TRUE(fixture.loop_.submit(docking, reason)) << reason;
EXPECT_TRUE(fixture.controller_.lastDockingMarker().empty());
}
TEST(ControlLoop, RejectsDockingRequestWhenMarkerIsUnknown)
{
Fixture fixture;
fixture.controller_.setDockingMarkerSucceeds(false); // marker không có trong maker_sources
NavigationRequest docking = makeRequest(1.0);
docking.profile = MotionProfile::kDocking;
docking.marker = "tram_la";
std::string reason;
EXPECT_FALSE(fixture.loop_.submit(docking, reason));
EXPECT_NE(reason.find("tram_la"), std::string::npos) << reason;
}
// ================================================================================================
@@ -329,7 +395,7 @@ TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce)
EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED");
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_EQ(fixture.mission_.reportCountFor(42), 1u)
<< "mỗi chặng chỉ được báo kết quả đúng một lần";
<< "each leg may report its outcome exactly once";
}
TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
@@ -343,7 +409,7 @@ TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer)
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_TRUE(fixture.mission_.reports().empty())
<< "goal trực tiếp không thuộc mission nào thì không báo lên mission layer";
<< "a direct goal belongs to no mission so nothing is reported to the mission layer";
}
TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling)
@@ -397,9 +463,9 @@ TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering)
if (move_base2::mustBeStopped(state))
{
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
<< "state " << move_base2::toString(state) << " ở cycle " << i;
<< "state " << move_base2::toString(state) << " at cycle " << i;
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0)
<< "state " << move_base2::toString(state) << " ở cycle " << i;
<< "state " << move_base2::toString(state) << " at cycle " << i;
}
if (!running)
@@ -454,7 +520,8 @@ TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan)
ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason;
fixture.run();
EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "không được đẩy plan rỗng xuống controller";
EXPECT_EQ(fixture.controller_.setPlanCount(), 0u) << "an empty plan must not be pushed down to "
"the controller";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}
@@ -476,13 +543,63 @@ TEST(ControlLoop, LostPoseStopsTheRobotImmediately)
fixture.loop_.step();
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0)
<< "mất TF thì phải dừng ngay, không đi tiếp bằng pose cũ";
<< "losing TF must stop the robot immediately, not keep going on a stale pose";
}
// ================================================================================================
// Recovery
// ================================================================================================
TEST(ControlLoop, PrimaryPlannerFailureUsesBackupBeforeRecovery)
{
ControlLoopConfig config = baseConfig();
config.backup_global_planner_name = "FakeBackupGlobalPlanner";
// 0 nghĩa là không retry planner hiện tại. Backup vẫn phải có đúng một lượt riêng trước recovery.
config.state_machine.max_planning_retries = 0;
Fixture fixture(config);
fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kOk});
fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached});
std::string reason;
NavigationRequest request = makeRequest(3.0, 71);
request.order = std::make_shared<robot_protocol_msgs::Order>();
ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason;
fixture.run();
EXPECT_FALSE(fixture.hitLimit());
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
EXPECT_EQ(fixture.planner_.orderHistory(), (std::vector<bool>{true, false}));
EXPECT_EQ(fixture.recovery_.startCount(), 0u);
EXPECT_EQ(fixture.states(),
(std::vector<std::string>{"IDLE", "PLANNING", "CONTROLLING", "SUCCEEDED"}))
<< join(fixture.states());
}
TEST(ControlLoop, BackupPlannerFailureEscalatesToRecovery)
{
ControlLoopConfig config = baseConfig();
config.backup_global_planner_name = "FakeBackupGlobalPlanner";
config.state_machine.max_planning_retries = 0;
Fixture fixture(config);
fixture.planner_.setScript({PlannerScript::kFail, PlannerScript::kFail});
fixture.recovery_.setScript({RecoveryScript::kRunning});
std::string reason;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 72), reason)) << reason;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kRecovering; ++i)
{
fixture.stepOnce();
}
EXPECT_EQ(fixture.planner_.activePlanner(), "FakeBackupGlobalPlanner");
EXPECT_EQ(fixture.planner_.makePlanCount(), 2u);
EXPECT_EQ(fixture.loop_.state(), NavigationState::kRecovering);
EXPECT_EQ(fixture.recovery_.startCount(), 1u);
EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed);
}
TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
{
// Kịch bản thật: planner bế tắc cho tới khi recovery gỡ được thế, sau đó lập plan bình thường.
@@ -521,7 +638,7 @@ TEST(ControlLoop, PlannerFailureDrivesRecoveryThenSucceeds)
fixture.clock_.advance(kControlPeriod);
}
EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery";
EXPECT_TRUE(planner_unblocked) << "never entered recovery";
EXPECT_EQ(states, (std::vector<std::string>{"IDLE", "PLANNING", "RECOVERING", "PLANNING",
"CONTROLLING", "SUCCEEDED"}))
<< join(states);
@@ -548,7 +665,7 @@ TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport)
<< join(fixture.states());
EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}))
<< "phải chạy lần lượt từng behavior, không lặp lại behavior đầu";
<< "behaviors must run one after another, the first one must not repeat";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u);
EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u);
@@ -566,7 +683,7 @@ TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior)
EXPECT_EQ(fixture.recovery_.startedIndices(), (std::vector<std::size_t>{0u, 1u}));
EXPECT_EQ(fixture.recovery_.updateCount(), 0u)
<< "không được tick một behavior chưa start thành công";
<< "a behavior that never started successfully must not be ticked";
EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED");
}
@@ -590,7 +707,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
fixture.loop_.lastCommand().linear.x < -1e-6)
{
saw_reverse = true;
EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "lệnh lùi phải nằm trong trần";
EXPECT_GE(fixture.loop_.lastCommand().linear.x, -0.2) << "the reverse command must stay "
"within the limit";
}
if (!running)
{
@@ -599,7 +717,8 @@ TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle)
fixture.clock_.advance(kControlPeriod);
}
EXPECT_TRUE(saw_reverse) << "recovery phát vận tốc nhưng lệnh không tới được đầu ra";
EXPECT_TRUE(saw_reverse) << "recovery published a velocity but the command never reached the "
"output";
}
TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure)
@@ -652,7 +771,7 @@ TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled)
}
}
EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ";
EXPECT_LE(cycles_to_zero, 2) << "cmd_vel must reach 0 within 2 cycles after a cancel";
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled);
EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED");
@@ -742,8 +861,8 @@ TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce)
}
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg;
ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg;
ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "leg " << leg;
ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "leg " << leg;
}
EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u);
@@ -813,7 +932,7 @@ TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce)
ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states());
EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted);
EXPECT_EQ(fixture.mission_.reportCountFor(13), 1u);
EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "action hỏng không được kéo recovery vào";
EXPECT_EQ(fixture.recovery_.startCount(), 0u) << "a failed action must not drag recovery in";
}
TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
@@ -833,7 +952,7 @@ TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing)
// Không goal lẫn action thì bị từ chối bất kể có port hay không.
EXPECT_FALSE(fixture.loop_.submit(makeActionOnlyRequest(0), reason));
EXPECT_NE(reason.find("goal lẫn action"), std::string::npos) << reason;
EXPECT_NE(reason.find("neither goal nor action"), std::string::npos) << reason;
}
// ================================================================================================
@@ -859,7 +978,7 @@ TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking)
{
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
<< "rời PLANNING khi planner còn đang tính, cycle " << i;
<< "left PLANNING while the planner was still computing, cycle " << i;
}
fixture.stepOnce();
@@ -882,7 +1001,7 @@ TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning)
}
EXPECT_EQ(fixture.planner_.makePlanCount(), 1u)
<< "lượt lập plan bị khởi động lại mỗi cycle";
<< "the planning attempt was restarted every cycle";
}
TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
@@ -908,7 +1027,7 @@ TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy)
// recovery -> ABORTED. Điều phải khoá lại là nó KHÔNG đứng im ở PLANNING.
EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"),
fixture.states().end())
<< "planner treo mà không ai escalate — robot đứng ở PLANNING vĩnh viễn: "
<< "the planner hung and nobody escalated — the robot would sit in PLANNING forever: "
<< join(fixture.states());
EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states());
}
@@ -935,7 +1054,7 @@ TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackgroun
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling);
EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9)
<< "cmd_vel gián đoạn trong lúc lập lại plan, cycle " << i;
<< "cmd_vel was interrupted while replanning, cycle " << i;
}
}
@@ -978,7 +1097,7 @@ TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan)
fixture.stepOnce();
EXPECT_GT(fixture.planner_.cancelCount(), before)
<< "yêu cầu mới không huỷ lượt lập plan của goal cũ";
<< "a new request did not cancel the planning attempt of the old goal";
}
TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt)
@@ -1026,8 +1145,8 @@ TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning)
<< "goal mới nằm chờ thay vì thay goal cũ ngay";
EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới";
<< "the new goal was queued instead of replacing the old one right away";
EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "did not replan for the new goal";
}
TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
@@ -1044,7 +1163,8 @@ TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ";
EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "the planning attempt for the old goal was not "
"cancelled";
}
TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
@@ -1063,8 +1183,10 @@ TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId)
ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason;
fixture.stepOnce();
EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần";
EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận";
EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "the preempted leg was not reported "
"exactly once";
EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "the NEW leg got its outcome reported the "
"moment it was accepted";
}
TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne)
@@ -1108,7 +1230,120 @@ TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior)
fixture.stepOnce();
EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning);
EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng";
EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "the running behavior was not told to stop";
}
// ================================================================================================
// Guard "không đi mù" — dữ liệu quan sát của costmap quá hạn
// ================================================================================================
//
// move_base thế hệ 1 có đúng guard này (`move_base.cpp:2720`) và move_base2 trước đây KHÔNG có:
// costmap hết hạn nghĩa là robot đang tránh vật cản trên một bản đồ của quá khứ.
TEST(StaleCostmap, BlocksWheelsWhileControlling)
{
Fixture fixture;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
// Chạy tới khi đang bám plan và thực sự có lệnh khác 0.
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.loop_.step();
ASSERT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0) << "no command to block yet";
const std::size_t controller_calls_before = fixture.controller_.computeCount();
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0) << "sensor data is stale yet it kept "
"driving";
EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0);
EXPECT_EQ(fixture.controller_.computeCount(), controller_calls_before)
<< "the controller must not compute a command on stale data";
EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling)
<< "the guard only blocks the wheels, it does not change state — exactly like the old "
"version";
}
TEST(StaleCostmap, ResumesWhenSensorDataBecomesCurrentAgain)
{
Fixture fixture;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0);
fixture.costmap_status_.setCurrent(true);
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
<< "sensors are fresh again yet the robot stays still";
}
TEST(StaleCostmap, DisabledByConfigLetsTheRobotDrive)
{
ControlLoopConfig config = baseConfig();
config.require_current_costmap = false;
Fixture fixture(config);
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
std::string error;
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false);
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0)
<< "the guard is disabled by config yet it still blocked";
}
TEST(StaleCostmap, NullPortMeansNoGuard)
{
// Đường đi của mọi test cổng-giả có sẵn: không ai bơm costmap_status thì lõi coi là còn hạn.
Fixture fixture;
fixture.deps_.costmap_status = nullptr;
std::string error;
ASSERT_TRUE(fixture.loop_.configure(baseConfig(), fixture.deps_, error)) << error;
fixture.planner_.setScript({ PlannerScript::kOk });
fixture.controller_.setScript(std::vector<ControllerScript>(50, ControllerScript::kOk));
ASSERT_TRUE(fixture.loop_.submit(makeRequest(2.0), error)) << error;
for (int i = 0; i < 20 && fixture.loop_.state() != NavigationState::kControlling; ++i)
{
fixture.loop_.step();
}
ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling);
fixture.costmap_status_.setCurrent(false); // không ai hỏi nó cả
fixture.loop_.step();
EXPECT_GT(std::abs(fixture.loop_.lastCommand().linear.x), 0.0);
EXPECT_EQ(fixture.costmap_status_.queryCount(), 0u) << "the port was detached yet the core still "
"asked it";
}
int main(int argc, char** argv)