diff --git a/CMakeLists.txt b/CMakeLists.txt index 174cc8c..51dd135 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 - $ - $ -) - -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 ) diff --git a/README.md b/README.md index 5ae8137..3b4f31d 100644 --- a/README.md +++ b/README.md @@ -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ứ đó. diff --git a/config/runtime/action_handlers_params.yaml b/config/runtime/action_handlers_params.yaml new file mode 100644 index 0000000..5aac630 --- /dev/null +++ b/config/runtime/action_handlers_params.yaml @@ -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 diff --git a/config/runtime/compound_actions_params.yaml b/config/runtime/compound_actions_params.yaml new file mode 100644 index 0000000..b346f3f --- /dev/null +++ b/config/runtime/compound_actions_params.yaml @@ -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: chặng chỉ-action; mang nguyên actionParameters của action gốc +# move_to: chặng nav, frame cố định +# move_to_param: chặng nav, frame lấy từ actionParameters[] của action gốc +# move: 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} diff --git a/config/runtime/maker_sources.yaml b/config/runtime/maker_sources.yaml deleted file mode 120000 index eac467e..0000000 --- a/config/runtime/maker_sources.yaml +++ /dev/null @@ -1 +0,0 @@ -../../../../pnkx_nav_core/config/maker_sources.yaml \ No newline at end of file diff --git a/config/runtime/maker_sources.yaml b/config/runtime/maker_sources.yaml new file mode 100644 index 0000000..6008ae0 --- /dev/null +++ b/config/runtime/maker_sources.yaml @@ -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 diff --git a/config/runtime/move_base_common_params.yaml b/config/runtime/move_base_common_params.yaml index b278cb3..d8b5ab7 100644 --- a/config/runtime/move_base_common_params.yaml +++ b/config/runtime/move_base_common_params.yaml @@ -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 diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md index 5e0e66f..7a8cb1e 100644 --- a/docs/ARCHITECTURE.md +++ b/docs/ARCHITECTURE.md @@ -50,15 +50,17 @@ 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 \ - src/AMR_T800/Test/move_base2/src +grep -rn "mission_adapters\|recovery_core\|action_core" src/AMR_T800/Test/move_base2/include \ + src/AMR_T800/Test/move_base2/src ``` ## Vì sao là port chứ không phải gọi thẳng @@ -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. diff --git a/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html b/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html new file mode 100644 index 0000000..9709f52 --- /dev/null +++ b/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html @@ -0,0 +1,184 @@ + + + + + move_base2: kiến trúc, C API và tương thích + + + +

move_base2: kiến trúc, C API và tương thích

+

Phân tích theo mã nguồn và log chạy của T800 · 03/08/2026

+ +
+ Kết luận ngắn. Khung chương trình T800 đang nạp move_base qua interface + robot::move_base_core::BaseNavigation và factory alias "MoveBase" có thể chạy + move_base2 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 + move_base. +
+ +

1. Khung đang khởi tạo những gì?

+

Log cho thấy chỉ có một node ROS là /amr_node cho phần điều khiển. Bên trong process đó, + amr_control nạp động navigation runtime và các plugin. Global planner, local planner, action, + recovery và mission adapter không là node ROS riêng.

+ +
+ roslaunch → /amr_node (amr_control) → BaseNavigation factory → NavigationServer (move_base2)
+ → costmap global/local + runners + mission/action/recovery plugins +
+ + + + + + + + + + + +
Thành phầnThời điểm / cách tạoVai trò quan sát được
amr_controlNode /amr_node; tạo TF, localization, sensor converter, publisher/subscriber rồi nạp navigation.Cầu nối ROS/MQTT/OPC-UA với lõi navigation.
NavigationServerFactory của libmove_base2.so trả về BaseNavigation::Ptr.Vỏ tương thích BaseNavigation; quản lý runtime và control thread 30 Hz.
Global/local costmapTạo khi NavigationRuntime::buildCostmaps() chạy.Global dùng frame map; local dùng odom; nhận laser, cloud, depth qua SensorGateway.
PlannerRunnerNạp CustomPlanner lúc boot; nạp SBPLLatticePlanner, DockPlanner khi profile cần.Lập global plan, cache instance theo tên planner.
ControllerRunnerNạp HybridLocalPlanner lúc boot; có thể đổi theo profile.Biến plan thành lệnh vận tốc.
RecoveryRunnerNạp lúc boot từ namespace recovery.Trong log: wait, clear-costmap (2 mức), rotate, back-up.
ActionRunnerNạp lúc boot từ namespace actions.Trong log: detect, wait, report, sim_noop.
Mission layerNạp source adapter lúc boot từ mission_adapters.GoalSourceAdapter nhận goal; VDA5050SourceAdapter tách order thành các leg/nav/action.
+ +

Luồng một VDA5050 order

+
    +
  1. MQTT nhận topic order.
    2. +
  2. VDA5050SourceAdapter đổi order thành các mission leg: navigation hoặc action-only.
    3. +
  3. MissionManager đưa leg vào hàng đợi; MissionExecutor chạy tuần tự.
    4. +
  4. Control loop chọn profile: position, docking, go_straight hoặc rotate; runner lấy planner/local planner phù hợp.
    5. +
  5. Kết quả nav/action trả về mission layer, rồi trạng thái VDA5050.
    6. +
+ +

2. C API đang dùng gì và có dùng được move_base2 không?

+

C API nằm tại pnkx_nav_core/src/APIs/c_api. Nó không tạo trực tiếp lớp C++ + move_base::MoveBase hay move_base2::NavigationServer. Hàm + navigation_create() làm đúng chuỗi sau:

+ 
PluginLoaderHelper::findLibraryPath("MoveBase")
+boost::dll::import_alias<BaseNavigation::Ptr()>(path, "MoveBase")
+factory()  -> NavigationHandle
+

Vì libmove_base2.so export cả hai alias MoveBase2 và MoveBase, + C API sẽ nhận được một NavigationServer của move_base2 khi cấu hình MoveBase trỏ đến + libmove_base2. C API phía gọi không cần đổi tên hàm.

+ + + + + + + +
Nhóm C APIVí dụKhi dùng move_base2
Vòng đờinavigation_create, navigation_initialize, navigation_destroyDùng được qua interface chung. Nên bảo đảm host gọi shutdown() ở đường C++ trước khi dỡ process/plugin.
Lệnh điều hướngnavigation_move_to, navigation_move_to_order, navigation_dock_to, pause/resume/cancelDùng được. Order/docking được move_base2 đưa vào mission/profile tương ứng.
Sensor và mapnavigation_add_static_map, navigation_add_laser_scan, navigation_add_point_cloud2, odometryDùng được; tên nguồn phải khớp source trong costmap YAML, ví dụ pc_r_marking.
Quan sátnavigation_get_feedback, pose/twist, global/local planner dataDùng được cho trạng thái BaseNavigation. Không tự lộ API chi tiết của MissionManager hay ActionRunner.
+ +
+ Lưu ý kỹ thuật C API: đây là ABI C-linkage (hàm dùng extern "C"), nhưng + header hiện có một số tham số tham chiếu C++ như PoseStamped &out_pose và + size_t &out_count. Vì vậy nó chưa là header ISO C thuần để 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. +
+ +

3. move_base2 khác move_base ở đâu?

+ + + + + + + + + +
Chủ đềmove_base cũ trong T800move_base2
Ranh giới với hostBaseNavigation, plugin được nạp qua alias MoveBase.Giữ cùng interface và xuất alias tương thích MoveBase; thêm alias rõ ràng MoveBase2.
Tổ chức runtimeĐiều phối kiểu đơn khối hơn.Tách NavigationRuntime, ControlLoop, PlannerRunner, ControllerRunner, RecoveryRunner, ActionRunner và MissionLayer.
Nhiệm vụ / orderHost hoặc lớp ngoài thường phải tự điều phối nhiều bước.Mission layer có adapter nguồn và executor; VDA5050 order được tách thành leg, action-only leg chạy qua ActionRunner.
ProfileThường chỉ một bộ planner/controller cho một kiểu goal.Profile position, docking, go_straight, rotate; docking có marker profile. Planner/local planner có thể chuyển theo mission.
FallbackPhụ thuộc implementation cũ.Log chứng minh: khi CustomPlanner không hỗ trợ request đơn giản, runtime chuyển một lần sang SBPLLatticePlanner.
SafetyTùy implementation.Có điều kiện require_current_costmap; sensor stale sẽ chặn wheel command để không điều khiển theo thế giới cũ.
Local planner pluginKhông nên giả định plugin cũ có cùng ABI.Dùng contract robot_nav_core2::LocalPlanner; local planner cũ chỉ dùng lại khi đã xác nhận cùng interface/ABI hoặc có adapter.
+ +
+ Đừng nhầm hai mức tương thích. Host/API BaseNavigation 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. +
+ +
+

4. Một khung đang dùng move_base có chạy move_base2 luôn không?

+

Câu trả lời: có điều kiện. Với amr_control của T800 thì câu trả lời là + có, theo đúng cơ chế đã được thiết kế. Với một khung ROS bất kỳ đang dùng package ROS1 + move_base, câu trả lời là không thể kết luận là có nếu chưa kiểm tra interface.

+ + + + + + + +
Loại khung hiện cóKhả năng chuyểnLý do / việc cần làm
T800 host nạp BaseNavigation::Ptr từ alias MoveBaseCaomove_base2 export alias tương thích. Đặt đúng library/config, rồi kiểm thử runtime.
Ứng dụng gọi C API navigation_create()CaoC API cũng tìm MoveBase; config quyết định .so được nạp. Cần cùng ABI của move_base_core.
Ứng dụng liên kết trực tiếp class/private header của move_base cũThấpPhả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.
ROS1 chuẩn dùng action move_base_msgs/MoveBaseAction và pluginlib/nav_coreChưa khẳng địnhĐây không phải tự động là contract T800 BaseNavigation; cần kiểm tra node/action/topic/plugin ABI cụ thể.
+ +

Điều kiện bắt buộc để chuyển khung T800

+
    +
  1. Chung ABI: host, libmove_base2.so và move_base_core phải được build từ cùng workspace/devel hoặc ABI tương thích.
    2. +
  2. Đúng alias: library phải export factory BaseNavigation::Ptr() dưới tên MoveBase. move_base2 hiện đã có alias này.
    3. +
  3. Đúng config: PNKX_NAV_CORE_CONFIG_DIR phải trỏ đến move_base2/config/runtime; file move_base_common_params.yaml cần có MoveBase: library_path: libmove_base2.
    4. +
  4. Đủ plugin: global planner/local planner/recovery/action/mission adapter được khai trong YAML phải có .so, alias factory và dependency đúng.
    5. +
  5. Đúng sensor contract: map, TF, odom, laser/cloud/depth được bơm bằng đúng topic-key và tần số. Với require_current_costmap: true, sensor stale sẽ chặn lệnh bánh xe.
    6. +
  6. Shutdown rõ ràng: dừng navigation trước khi destroy loader/process để tránh race thread/plugin.
    7. +
+ +

Cách chuyển an toàn trong launch của T800

+ 
<!-- move_base2_control.launch đã làm hai việc quan trọng -->
+<env name="PNKX_NAV_CORE_CONFIG_DIR"
+     value="$(find move_base2)/config/runtime" />
+
+# config/runtime/move_base_common_params.yaml
+MoveBase:
+  library_path: libmove_base2
+

Không đổi trực tiếp link library trong amr_control. Nó tiếp tục nạp factory + "MoveBase"; file cấu hình chọn implementation là move_base2.

+ +

5. Checklist test trước khi thay cho hệ chạy dài

+
    +
  1. Build: xác nhận có devel/lib/libmove_base2.so và các plugin runtime.
    2. +
  2. Boot: log phải có Found library ... libmove_base2.so, NavigationRuntime built, costmap và sensor source tạo thành công.
    3. +
  3. Goal đơn: position, cancel/preempt, pause/resume; xác nhận cmd_vel và feedback.
    4. +
  4. Order: VDA5050 order có nhiều node/edge, action wait/detect/charge và docking marker.
    5. +
  5. 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.
    6. +
  6. 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.
    7. +
+ +

6. Liên hệ với cảnh báo qua đêm trong log

+

Cảnh báo /gazebo/default/pose/local/info 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à:

+ 
/camera_right/depth/points_proc observation buffer has not been updated ...
+[move_base2] Sensor data is stale — wheel commands blocked
+

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.

+ +

7. Dấu vết mã nguồn dùng để kết luận

+
    +
  • Controllers/Packages/amr_control/src/amr_control.cpp: host tìm library MoveBase và import factory alias.
    • +
  • Test/move_base2/src/move_base2_plugin.cpp: factory trả BaseNavigation::Ptr, export cả MoveBase2 và MoveBase.
    • +
  • Test/move_base2/launch/move_base2_control.launch: đặt PNKX_NAV_CORE_CONFIG_DIR sang overlay runtime.
    • +
  • Test/move_base2/config/runtime/move_base_common_params.yaml: MoveBase.library_path = libmove_base2.
    • +
  • pnkx_nav_core/src/APIs/c_api/src/nav_c_api.cpp: navigation_create() dùng chính alias MoveBase.
    • +
  • pnkx_nav_core/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h: contract chung BaseNavigation.
    • +
  • Test/move_base2/src/navigation_runtime.cpp: xây costmap, runners và mission layer.
    • +
+ + diff --git a/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf b/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf new file mode 100644 index 0000000..04ea3d5 Binary files /dev/null and b/docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf differ diff --git a/docs/STATE_MACHINE.md b/docs/STATE_MACHINE.md index 54505f8..95fd39a 100644 --- a/docs/STATE_MACHINE.md +++ b/docs/STATE_MACHINE.md @@ -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ể: diff --git a/include/move_base2/bridges/mission_adapter_bridge.h b/include/move_base2/bridges/mission_adapter_bridge.h index e968044..e0bb822 100644 --- a/include/move_base2/bridges/mission_adapter_bridge.h +++ b/include/move_base2/bridges/mission_adapter_bridge.h @@ -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: diff --git a/include/move_base2/bridges/mission_layer.h b/include/move_base2/bridges/mission_layer.h new file mode 100644 index 0000000..8fbb01b --- /dev/null +++ b/include/move_base2/bridges/mission_layer.h @@ -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 +#include + +#include + +#include +#include +#include +#include +#include + +#include + +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_ diff --git a/include/move_base2/config/move_base2_config.h b/include/move_base2/config/move_base2_config.h index 01e3e38..71c02c1 100644 --- a/include/move_base2/config/move_base2_config.h +++ b/include/move_base2/config/move_base2_config.h @@ -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 (`/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. diff --git a/include/move_base2/control_loop.h b/include/move_base2/control_loop.h index 81d4e65..ee4c3e4 100644 --- a/include/move_base2/control_loop.h +++ b/include/move_base2/control_loop.h @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include 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; + /** * @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 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. * diff --git a/include/move_base2/core/navigation_request.h b/include/move_base2/core/navigation_request.h index 738fcd3..e161684 100644 --- a/include/move_base2/core/navigation_request.h +++ b/include/move_base2/core/navigation_request.h @@ -10,6 +10,7 @@ #define MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_ #include +#include #include #include #include @@ -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 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::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. * diff --git a/include/move_base2/core/state_machine.h b/include/move_base2/core/state_machine.h index 8c9f9fa..afc989e 100644 --- a/include/move_base2/core/state_machine.h +++ b/include/move_base2/core/state_machine.h @@ -9,6 +9,7 @@ #ifndef MOVE_BASE2_CORE_STATE_MACHINE_H_ #define MOVE_BASE2_CORE_STATE_MACHINE_H_ +#include #include #include @@ -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 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. diff --git a/include/move_base2/io/costmap_exporter.h b/include/move_base2/io/costmap_exporter.h index 0046d07..036b4e7 100644 --- a/include/move_base2/io/costmap_exporter.h +++ b/include/move_base2/io/costmap_exporter.h @@ -13,6 +13,7 @@ #include #include +#include #include 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_; diff --git a/include/move_base2/io/runtime_stats.h b/include/move_base2/io/runtime_stats.h new file mode 100644 index 0000000..e12f522 --- /dev/null +++ b/include/move_base2/io/runtime_stats.h @@ -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//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 +#include +#include +#include +#include +#include + +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(-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 listThreadIds(); + + const double period_seconds_; + const double ticks_per_second_; + + mutable std::mutex mutex_; + + std::vector
sections_; + std::vector threads_; + std::vector 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(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_ diff --git a/include/move_base2/io/sensor_gateway.h b/include/move_base2/io/sensor_gateway.h index c555b82..d2753d1 100644 --- a/include/move_base2/io/sensor_gateway.h +++ b/include/move_base2/io/sensor_gateway.h @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -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_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 diff --git a/include/move_base2/navigation_runtime.h b/include/move_base2/navigation_runtime.h index 64ee417..386c774 100644 --- a/include/move_base2/navigation_runtime.h +++ b/include/move_base2/navigation_runtime.h @@ -11,11 +11,14 @@ #include #include +#include #include #include +#include #include +#include #include #include #include @@ -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& 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 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& 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 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 global_costmap_; std::unique_ptr 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 global_footprint_; + std::vector 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 diff --git a/include/move_base2/navigation_server.h b/include/move_base2/navigation_server.h index f0cb2ec..30403bf 100644 --- a/include/move_base2/navigation_server.h +++ b/include/move_base2/navigation_server.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -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 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 control_thread_running_{ false }; robot::TFListenerPtr tf_; @@ -293,6 +320,7 @@ private: mutable std::mutex data_mutex_; std::vector footprint_; + bool footprint_pending_ = false; std::map 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_; }; diff --git a/include/move_base2/ports/controller_port.h b/include/move_base2/ports/controller_port.h index 0c7a823..b785333 100644 --- a/include/move_base2/ports/controller_port.h +++ b/include/move_base2/ports/controller_port.h @@ -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. diff --git a/include/move_base2/ports/costmap_status_port.h b/include/move_base2/ports/costmap_status_port.h new file mode 100644 index 0000000..55dfb8e --- /dev/null +++ b/include/move_base2/ports/costmap_status_port.h @@ -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_ diff --git a/include/move_base2/ports/pose_port.h b/include/move_base2/ports/pose_port.h index d46318c..0ce802c 100644 --- a/include/move_base2/ports/pose_port.h +++ b/include/move_base2/ports/pose_port.h @@ -9,6 +9,8 @@ #ifndef MOVE_BASE2_PORTS_POSE_PORT_H_ #define MOVE_BASE2_PORTS_POSE_PORT_H_ +#include + #include 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 diff --git a/include/move_base2/ports/recovery_port.h b/include/move_base2/ports/recovery_port.h index 6e32744..4f576f9 100644 --- a/include/move_base2/ports/recovery_port.h +++ b/include/move_base2/ports/recovery_port.h @@ -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 planning_failed; + std::vector controlling_failed; + std::vector oscillation; + + const std::vector& forTrigger(RecoveryTrigger trigger) const; + bool empty() const; +}; + /** * @enum RecoveryOutputKind * @brief Behavior đó có lái robot hay không. diff --git a/include/move_base2/runners/action_handler.h b/include/move_base2/runners/action_handler.h deleted file mode 100644 index 2c019e6..0000000 --- a/include/move_base2/runners/action_handler.h +++ /dev/null @@ -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 -#include -#include - -// 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 - -#include -#include -#include - -#include - -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; - - 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 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_ diff --git a/include/move_base2/runners/action_runner.h b/include/move_base2/runners/action_runner.h index 6f9318c..a45fe8c 100644 --- a/include/move_base2/runners/action_runner.h +++ b/include/move_base2/runners/action_runner.h @@ -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 -#include -#include #include #include +#include + #include #include -#include 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 `/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 supportedActionTypes() const; + std::vector 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 handlers_; - std::map 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> factories_; }; } // namespace move_base2 diff --git a/include/move_base2/runners/controller_runner.h b/include/move_base2/runners/controller_runner.h index 0320d27..acca87c 100644 --- a/include/move_base2/runners/controller_runner.h +++ b/include/move_base2/runners/controller_runner.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -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& 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 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 active_plan_; + /** * Trần vận tốc và vận tốc đo được gần nhất. * diff --git a/include/move_base2/runners/planner_runner.h b/include/move_base2/runners/planner_runner.h index e1f40b5..1f166e3 100644 --- a/include/move_base2/runners/planner_runner.h +++ b/include/move_base2/runners/planner_runner.h @@ -24,6 +24,7 @@ #include #include +#include #include 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 planners_; std::string active_name_; diff --git a/include/move_base2/runners/recovery_runner.h b/include/move_base2/runners/recovery_runner.h index 269db09..72835b4 100644 --- a/include/move_base2/runners/recovery_runner.h +++ b/include/move_base2/runners/recovery_runner.h @@ -92,6 +92,20 @@ public: */ bool configure(robot::NodeHandle& nh) override; + /** + * @brief Resolve YAML `/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_ }; diff --git a/launch/move_base2_control.launch b/launch/move_base2_control.launch index 29d9633..8554134 100644 --- a/launch/move_base2_control.launch +++ b/launch/move_base2_control.launch @@ -26,7 +26,8 @@ - + + @@ -45,7 +46,8 @@ - + diff --git a/package.xml b/package.xml index d894fa7..19a518a 100644 --- a/package.xml +++ b/package.xml @@ -82,4 +82,8 @@ mission_adapters mission_adapters + + action_core + action_core + diff --git a/plugins/noop_action_handler.cpp b/plugins/noop_action_handler.cpp deleted file mode 100644 index 3e5f7a5..0000000 --- a/plugins/noop_action_handler.cpp +++ /dev/null @@ -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 - -#include -#include -#include -#include - -#include -#include - -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(); - } - - 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{"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 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 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) diff --git a/src/bridges/mission_adapter_bridge.cpp b/src/bridges/mission_adapter_bridge.cpp index 5d63dfd..0937080 100644 --- a/src/bridges/mission_adapter_bridge.cpp +++ b/src/bridges/mission_adapter_bridge.cpp @@ -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(); + 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(mission->id)); + robot::log_warning("[move_base2] MissionAdapterBridge: rejecting mission %llu — the bridge did " + "not start.\n", static_cast(mission->id)); return false; } @@ -97,8 +149,8 @@ bool MissionAdapterBridge::dispatch(const std::shared_ptr(mission->id), + robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu overwrites mission %llu " + "before it was pushed down.\n", static_cast(mission->id), static_cast(pending_->id)); } diff --git a/src/bridges/mission_layer.cpp b/src/bridges/mission_layer.cpp new file mode 100644 index 0000000..e81a74f --- /dev/null +++ b/src/bridges/mission_layer.cpp @@ -0,0 +1,190 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt MissionLayer. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +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 diff --git a/src/config/move_base2_config.cpp b/src/config/move_base2_config.cpp index 2ea2c14..21a8b26 100644 --- a/src/config/move_base2_config.cpp +++ b/src/config/move_base2_config.cpp @@ -9,6 +9,7 @@ #include #include +#include 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(); + 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(); + binding.local_planner_name = value["local_planner"].as(); + 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á `_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; } diff --git a/src/control_loop.cpp b/src/control_loop.cpp index 073626f..647118a 100644 --- a/src/control_loop.cpp +++ b/src/control_loop.cpp @@ -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(request.mission_sequence_id), + toString(request.profile), request.marker.empty() ? "" : 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(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(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(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(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(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(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(outgoing_mission_id), last_reason_ != nullptr ? last_reason_ : ""); break; diff --git a/src/io/costmap_exporter.cpp b/src/io/costmap_exporter.cpp index 86b921f..d4cc74b 100644 --- a/src/io/costmap_exporter.cpp +++ b/src/io/costmap_exporter.cpp @@ -107,12 +107,21 @@ void CostmapExporter::prepareGridLocked() } } +void CostmapExporter::attachTelemetry(RuntimeStats* telemetry) +{ + std::lock_guard 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 lock(mutex_); + ScopedSection timer(telemetry_, section_fill_); if (costmap_ == nullptr) { return; diff --git a/src/io/runtime_stats.cpp b/src/io/runtime_stats.cpp new file mode 100644 index 0000000..9e9bd9a --- /dev/null +++ b/src/io/runtime_stats.cpp @@ -0,0 +1,425 @@ +/** + * @file runtime_stats.cpp + * @brief Hiện thực @ref move_base2::RuntimeStats. + */ +#include + +#include +#include +#include +#include +#include + +#ifdef __linux__ +#include +#include +#include +#endif + +#include + +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(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(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(page_size > 0 ? page_size : 4096); +#else + return 0; +#endif +} + +std::vector RuntimeStats::listThreadIds() +{ + std::vector 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 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 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(syscall(SYS_gettid)); +#else + const long tid = 0; +#endif + + std::lock_guard 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 lock(mutex_); + capture_before_ = listThreadIds(); + capturing_ = true; +} + +void RuntimeStats::endThreadCapture(const std::string& label) +{ + if (!enabled()) + { + return; + } + + std::lock_guard 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 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(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 lock(mutex_); + + const auto now = std::chrono::steady_clock::now(); + const double window = std::chrono::duration(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(process_ticks - last_process_cpu_ticks_) / ticks_per_second_ / safe_window; + const double rss_mb = static_cast(rss_bytes) / (1024.0 * 1024.0); + const double rss_delta_mb = (static_cast(rss_bytes) - static_cast(last_rss_bytes_)) / (1024.0 * 1024.0); + + const std::vector 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(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(section.calls) / safe_window; + const double avg_ms = + section.calls > 0 ? static_cast(section.total_ns) / static_cast(section.calls) / 1e6 : 0.0; + const double max_ms = static_cast(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(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 diff --git a/src/io/sensor_gateway.cpp b/src/io/sensor_gateway.cpp index 14cb0ab..aa45e06 100644 --- a/src/io/sensor_gateway.cpp +++ b/src/io/sensor_gateway.cpp @@ -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_); diff --git a/src/navigation_runtime.cpp b/src/navigation_runtime.cpp index e36eaba..57926f2 100644 --- a/src/navigation_runtime.cpp +++ b/src/navigation_runtime.cpp @@ -12,11 +12,48 @@ #include #include +#include #include 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& 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& 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& 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& 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; } diff --git a/src/navigation_server.cpp b/src/navigation_server.cpp index 91e7185..0c202b8 100644 --- a/src/navigation_server.cpp +++ b/src/navigation_server.cpp @@ -34,7 +34,7 @@ NavigationServer::NavigationServer() // ngay từ lúc dựng — trước cả initialize(). nav_feedback_ = std::make_shared(); 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(); - cachePlans(); + { + 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(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 lock(data_mutex_); footprint_ = fprt; + footprint_pending_ = true; } std::vector NavigationServer::getRobotFootprint() @@ -390,6 +472,31 @@ std::vector NavigationServer::getRobotFootprint() return footprint_; } +void NavigationServer::applyPendingFootprint() +{ + if (runtime_ == nullptr) + { + return; + } + + std::vector footprint; + { + std::lock_guard 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(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(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 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 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 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) diff --git a/src/navigation_state.cpp b/src/navigation_state.cpp index e16535e..6251d8c 100644 --- a/src/navigation_state.cpp +++ b/src/navigation_state.cpp @@ -104,6 +104,25 @@ const char* toString(RecoveryTrigger trigger) return "unknown"; } +const std::vector& 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) diff --git a/src/runners/action_runner.cpp b/src/runners/action_runner.cpp index 232dfe6..18783f8 100644 --- a/src/runners/action_runner.cpp +++ b/src/runners/action_runner.cpp @@ -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 -#include - -#include -#include -#include +#include #include @@ -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; - } - - const std::vector types = handler->supportedActionTypes(); - if (types.empty()) - { - robot::log_error("[move_base2] ActionRunner: handler không khai actionType nào — sẽ không bao " - "giờ được gọi."); - return false; - } - - for (const std::string& type : types) - { - if (type.empty()) - { - robot::log_error("[move_base2] ActionRunner: handler khai một actionType rỗng."); - return false; - } - if (by_type_.find(type) != by_type_.end()) - { - // Hai handler cùng nhận một type thì việc định tuyến phụ thuộc thứ tự nạp — từ chối thay vì - // im lặng ghi đè. - robot::log_error("[move_base2] ActionRunner: actionType '%s' đã có handler khác đăng ký.", - type.c_str()); - return false; - } - } - - handlers_.push_back(handler); - for (const std::string& type : types) - { - by_type_[type] = handler.get(); - } - return true; + context_ = context; } -ActionHandler* ActionRunner::find(const std::string& action_type) const +bool ActionRunner::registerHandler(const action_core::ActionHandler::Ptr& handler) { - const auto it = by_type_.find(action_type); - return it == by_type_.end() ? nullptr : it->second; + return registry_.registerHandler(handler); } -std::vector ActionRunner::supportedActionTypes() const +ActionTick ActionRunner::toTick(const action_core::ActionTick& tick) { - std::vector types; - types.reserve(by_type_.size()); - for (const auto& entry : by_type_) - { - types.push_back(entry.first); - } - return types; -} + ActionTick out; + out.message = tick.message; -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()) + switch (tick.status) { - 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; + 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; } - std::function factory; - try - { - factory = boost::dll::import_alias( - 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"; - - YAML::Node list; - if (!nh.getParam(key, list) || !list.IsSequence()) - { - // Không có handler nào là hợp lệ: hệ không có thiết bị thì mọi mission đều nav-only, và - // ControlLoop::submit đã từ chối yêu cầu mang action ngay tại cửa. - robot::log_warning("[move_base2] ActionRunner: '%s' không có danh sách handler — runtime sẽ " - "từ chối mọi yêu cầu mang action.", key.c_str()); - configured_ = true; - return true; - } - - bool all_ok = true; - - for (std::size_t i = 0; i < list.size(); ++i) - { - const YAML::Node& entry = list[i]; - - if (!entry.IsMap() || !entry["type"]) - { - robot::log_error("[move_base2] ActionRunner: '%s[%zu]' thiếu khoá 'type'.", key.c_str(), i); - all_ok = false; - continue; - } - - std::string type; - std::string name; - try - { - type = entry["type"].as(); - name = entry["name"] ? entry["name"].as() : 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; - } - } + registry_.setContext(context_); + const bool ok = registry_.loadFromConfig(nh, namespace_); 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() ? "" : 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) { diff --git a/src/runners/controller_runner.cpp b/src/runners/controller_runner.cpp index e68db30..91108b3 100644 --- a/src/runners/controller_runner.cpp +++ b/src/runners/controller_runner.cpp @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -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& 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& 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::vectorsetGoalPose(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 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; } diff --git a/src/runners/planner_runner.cpp b/src/runners/planner_runner.cpp index ba8cffe..163a9e7 100644 --- a/src/runners/planner_runner.cpp +++ b/src/runners/planner_runner.cpp @@ -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 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; } diff --git a/src/runners/recovery_runner.cpp b/src/runners/recovery_runner.cpp index c349424..b4b231b 100644 --- a/src/runners/recovery_runner.cpp +++ b/src/runners/recovery_runner.cpp @@ -5,9 +5,12 @@ *********************************************************************/ #include +#include +#include #include #include +#include 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 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 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(); + 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* 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 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(); + 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; } diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 41d7d3b..d2bbb2a 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -8,6 +8,7 @@ *********************************************************************/ #include +#include #include 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& 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& 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& 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; } diff --git a/src/velocity_arbiter.cpp b/src/velocity_arbiter.cpp index 9deb0dd..ef54248 100644 --- a/src/velocity_arbiter.cpp +++ b/src/velocity_arbiter.cpp @@ -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"; diff --git a/test/action_runner_test.cpp b/test/action_runner_test.cpp index 35e3048..62a5792 100644 --- a/test/action_runner_test.cpp +++ b/test/action_runner_test.cpp @@ -13,13 +13,14 @@ #include +#include #include #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) diff --git a/test/config/move_base2_params.yaml b/test/config/move_base2_params.yaml index 847177f..a6b1814 100644 --- a/test/config/move_base2_params.yaml +++ b/test/config/move_base2_params.yaml @@ -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 diff --git a/test/config_validation_test.cpp b/test/config_validation_test.cpp index 9e2f65c..21506fe 100644 --- a/test/config_validation_test.cpp +++ b/test/config_validation_test.cpp @@ -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) diff --git a/test/controller_runner_test.cpp b/test/controller_runner_test.cpp index 2ace7ce..206a083 100644 --- a/test/controller_runner_test.cpp +++ b/test/controller_runner_test.cpp @@ -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); diff --git a/test/fake_ports.h b/test/fake_ports.h index a44136d..5786ef6 100644 --- a/test/fake_ports.h +++ b/test/fake_ports.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -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& 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 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& 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: diff --git a/test/mission_adapter_bridge_test.cpp b/test/mission_adapter_bridge_test.cpp index 6de85e2..37841db 100644 --- a/test/mission_adapter_bridge_test.cpp +++ b/test/mission_adapter_bridge_test.cpp @@ -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) diff --git a/test/mission_layer_test.cpp b/test/mission_layer_test.cpp new file mode 100644 index 0000000..d79f110 --- /dev/null +++ b/test/mission_layer_test.cpp @@ -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 + +#include +#include +#include +#include +#include + +#include +#include + +#include + +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->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(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(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 received_; + int cancel_calls_ = 0; + std::shared_ptr 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(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(); +} diff --git a/test/move_base2_scenario_driver.h b/test/move_base2_scenario_driver.h index f733952..36f0a78 100644 --- a/test/move_base2_scenario_driver.h +++ b/test/move_base2_scenario_driver.h @@ -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; diff --git a/test/move_base2_scenario_test.cpp b/test/move_base2_scenario_test.cpp index 96885df..58f3bb8 100644 --- a/test/move_base2_scenario_test.cpp +++ b/test/move_base2_scenario_test.cpp @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -19,10 +20,12 @@ #include #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 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(); } diff --git a/test/navigation_server_test.cpp b/test/navigation_server_test.cpp index 5715756..db39bab 100644 --- a/test/navigation_server_test.cpp +++ b/test/navigation_server_test.cpp @@ -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) diff --git a/test/planner_runner_test.cpp b/test/planner_runner_test.cpp index ef4585b..19e46d1 100644 --- a/test/planner_runner_test.cpp +++ b/test/planner_runner_test.cpp @@ -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) diff --git a/test/plugins/test_global_planner.cpp b/test/plugins/test_global_planner.cpp index 8379a61..643df90 100644 --- a/test/plugins/test_global_planner.cpp +++ b/test/plugins/test_global_planner.cpp @@ -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(); diff --git a/test/plugins/test_local_planner.cpp b/test/plugins/test_local_planner.cpp index 915bce3..d02f7db 100644 --- a/test/plugins/test_local_planner.cpp +++ b/test/plugins/test_local_planner.cpp @@ -19,6 +19,7 @@ * Author: DuongTD *********************************************************************/ #include +#include #include #include #include @@ -51,23 +52,32 @@ class TestLocalPlanner : public robot_nav_core2::LocalPlanner public: enum class Behavior { - kOk, ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được. - 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). + kOk, ///< Sinh lệnh hợp lệ, phản ánh trần và vận tốc đo được. + 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). + 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 /*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::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 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::Behavior::kRefusesLimits); } +robot_nav_core2::LocalPlanner::Ptr createMarkerProbe() +{ + return std::make_shared(TestLocalPlanner::Behavior::kMarkerProbe); +} + +robot_nav_core2::LocalPlanner::Ptr createFootprintProbe() +{ + return std::make_shared(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) diff --git a/test/recovery_runner_test.cpp b/test/recovery_runner_test.cpp index 26f0ce8..b8be6b8 100644 --- a/test/recovery_runner_test.cpp +++ b/test/recovery_runner_test.cpp @@ -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) diff --git a/test/recovery_scenario_driver.h b/test/recovery_scenario_driver.h new file mode 100644 index 0000000..f4f8835 --- /dev/null +++ b/test/recovery_scenario_driver.h @@ -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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#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 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 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 costmap_; + std::unique_ptr checker_; + nav_test_harness::FakePoseProvider pose_source_; + std::unique_ptr pose_bridge_; + std::unique_ptr 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 registry_; + std::unique_ptr 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_ diff --git a/test/runtime_stats_test.cpp b/test/runtime_stats_test.cpp new file mode 100644 index 0000000..2816dbc --- /dev/null +++ b/test/runtime_stats_test.cpp @@ -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 + +#include +#include +#include + +#include + +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(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 registered{ false }; + std::atomic stop{ false }; + std::thread worker([&stats, ®istered, &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 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(); +} diff --git a/test/sensor_gateway_test.cpp b/test/sensor_gateway_test.cpp index 4363bed..e282ffd 100644 --- a/test/sensor_gateway_test.cpp +++ b/test/sensor_gateway_test.cpp @@ -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); } diff --git a/test/spy_layer.h b/test/spy_layer.h index 87c3a65..e240f65 100644 --- a/test/spy_layer.h +++ b/test/spy_layer.h @@ -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_) diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp index 3f3c2f7..64cb50c 100644 --- a/test/state_machine_test.cpp +++ b/test/state_machine_test.cpp @@ -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) diff --git a/test/velocity_arbiter_test.cpp b/test/velocity_arbiter_test.cpp index 872b9bf..fe811bd 100644 --- a/test/velocity_arbiter_test.cpp +++ b/test/velocity_arbiter_test.cpp @@ -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) diff --git a/test/walking_skeleton_test.cpp b/test/walking_skeleton_test.cpp index b611c47..6a1728c 100644 --- a/test/walking_skeleton_test.cpp +++ b/test/walking_skeleton_test.cpp @@ -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(); + 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{true, false})); + EXPECT_EQ(fixture.recovery_.startCount(), 0u); + EXPECT_EQ(fixture.states(), + (std::vector{"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{"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{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{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(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(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(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(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)