From 701d25f95264e02dcedbd4ebad5cbf87d8cae99b Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 3 Aug 2026 22:41:32 +0700 Subject: [PATCH] optimal & fix file cmake --- CMakeLists.txt | 64 +- README.md | 17 +- config/runtime/action_handlers_params.yaml | 77 +++ config/runtime/compound_actions_params.yaml | 54 ++ config/runtime/maker_sources.yaml | 24 +- config/runtime/move_base_common_params.yaml | 98 ++-- docs/ARCHITECTURE.md | 52 +- docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html | 184 ++++++ docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf | Bin 0 -> 87981 bytes docs/STATE_MACHINE.md | 57 +- .../bridges/mission_adapter_bridge.h | 4 +- include/move_base2/bridges/mission_layer.h | 179 ++++++ include/move_base2/config/move_base2_config.h | 76 ++- include/move_base2/control_loop.h | 65 +- include/move_base2/core/navigation_request.h | 45 +- include/move_base2/core/state_machine.h | 12 +- include/move_base2/io/costmap_exporter.h | 9 + include/move_base2/io/runtime_stats.h | 200 +++++++ include/move_base2/io/sensor_gateway.h | 17 + include/move_base2/navigation_runtime.h | 95 +++ include/move_base2/navigation_server.h | 38 ++ include/move_base2/ports/controller_port.h | 20 +- .../move_base2/ports/costmap_status_port.h | 48 ++ include/move_base2/ports/pose_port.h | 19 + include/move_base2/ports/recovery_port.h | 22 + include/move_base2/runners/action_handler.h | 79 --- include/move_base2/runners/action_runner.h | 67 ++- .../move_base2/runners/controller_runner.h | 34 +- include/move_base2/runners/planner_runner.h | 14 + include/move_base2/runners/recovery_runner.h | 16 + launch/move_base2_control.launch | 6 +- package.xml | 4 + plugins/noop_action_handler.cpp | 171 ------ src/bridges/mission_adapter_bridge.cpp | 76 ++- src/bridges/mission_layer.cpp | 190 ++++++ src/config/move_base2_config.cpp | 325 ++++++---- src/control_loop.cpp | 204 ++++++- src/io/costmap_exporter.cpp | 9 + src/io/runtime_stats.cpp | 425 ++++++++++++++ src/io/sensor_gateway.cpp | 43 +- src/navigation_runtime.cpp | 196 ++++++- src/navigation_server.cpp | 220 ++++++- src/navigation_state.cpp | 19 + src/runners/action_runner.cpp | 238 ++------ src/runners/controller_runner.cpp | 254 ++++++-- src/runners/planner_runner.cpp | 62 +- src/runners/recovery_runner.cpp | 187 +++++- src/state_machine.cpp | 182 ++++-- src/velocity_arbiter.cpp | 19 +- test/action_runner_test.cpp | 17 +- test/config/move_base2_params.yaml | 93 ++- test/config_validation_test.cpp | 85 ++- test/controller_runner_test.cpp | 78 ++- test/fake_ports.h | 70 ++- test/mission_adapter_bridge_test.cpp | 81 ++- test/mission_layer_test.cpp | 368 ++++++++++++ test/move_base2_scenario_driver.h | 33 +- test/move_base2_scenario_test.cpp | 46 +- test/navigation_server_test.cpp | 42 +- test/planner_runner_test.cpp | 14 +- test/plugins/test_global_planner.cpp | 2 +- test/plugins/test_local_planner.cpp | 50 +- test/recovery_runner_test.cpp | 44 +- test/recovery_scenario_driver.h | 555 ++++++++++++++++++ test/runtime_stats_test.cpp | 184 ++++++ test/sensor_gateway_test.cpp | 6 +- test/spy_layer.h | 2 +- test/state_machine_test.cpp | 115 +++- test/velocity_arbiter_test.cpp | 14 +- test/walking_skeleton_test.cpp | 303 ++++++++-- 70 files changed, 5572 insertions(+), 1146 deletions(-) create mode 100644 config/runtime/action_handlers_params.yaml create mode 100644 config/runtime/compound_actions_params.yaml mode change 120000 => 100644 config/runtime/maker_sources.yaml create mode 100644 docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.html create mode 100644 docs/MOVE_BASE2_RUNTIME_VA_TUONG_THICH.pdf create mode 100644 include/move_base2/bridges/mission_layer.h create mode 100644 include/move_base2/io/runtime_stats.h create mode 100644 include/move_base2/ports/costmap_status_port.h delete mode 100644 include/move_base2/runners/action_handler.h delete mode 100644 plugins/noop_action_handler.cpp create mode 100644 src/bridges/mission_layer.cpp create mode 100644 src/io/runtime_stats.cpp create mode 100644 test/mission_layer_test.cpp create mode 100644 test/recovery_scenario_driver.h create mode 100644 test/runtime_stats_test.cpp 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 0000000000000000000000000000000000000000..04ea3d59fb2f2e4db78678895aa9712a624c7103 GIT binary patch literal 87981 zcma&tQ;=p$yDsdqZC96V+kDHmx@_CFZQHhO8(p@|-gBhk&t{otQK3AgR$)|NoM&NdRA6jjU14G7P_1WVT5e+e zXH5DSW=(1Q5Jm+Es0{_G7RJQZ`2SA+r}giCF#m4zZ-b5T-@*UtV*8J?u>U`tWd(Pl zlBUDOudi?w-qo2k1#I-sKKFc=9Dy-%BY=DU2%?kY`;#kZv~kk4gD2q5{aBk?%EkGl zVws>+U%)qax%fpoY(lJUttl%o#OIFj`yno_&-ZY8a9PB=!Avo+^gjKxGD86JER%XRLX};Zv%sn69 zw;*k8%dqz%apm`Z_`mldQ2U>*$NC7+@ScUvN$`>jqWEr3f*d8h6n@%=E@k{6vKz2Y z$r9)(Iynh=3EkSa#2{#|L;ZBmAEeT>KOwRaVSdel%fiYv=1>3O7y^c3ulIn%+xd9V zOF!qICp!UnI&yz}Q3!A^9V^|6^z@}Zf32O~+wJxKefxU32Tf!fyn?P;*vjyAZtgdJ zmQ2~m>>+RL^l7Jv9IWu~>$kdFgUo|wIi#a^^U8uQqEKizA)Y~iT1Yi-ih|B@c0ESb zjqQpL4T=os8(n`#A0MzZiUi}He(QpF-Rytv7Xjr>IwPO=gJIAhr(1Upjei>_Xci`4 zcHHKjIZiqdv&u)26ykSqY&6Cb{m;x{lh2TI^C3JEFM*E={?rX@qGt@&? z8e0HnB28SJTOx)d&$cm!g7Ma)+x5Z{I!`d$3yISv?YF%CWz12R*S|BN4xZ1=agV8S z-gMY(SEi~dVQus{gIq$tDhUnJX0A9rA{ zr$s)B++v3br6g8|gKFlYhXqwopaxGRnLC%`qLyycb5dPJXgA(UHwhc3p`)43z9(r% z?Hf`N|OC!_D!8suCqy z*oKSB%;Fh9%$}v;<%@^Rg=1<;OkwGlAgqs4by|2M+jqN_3#%Bh`vwX4GXpq68Ply+ zlonSt`3exVV{X}u0$V*4ekkE$?4Jg{;$-h$<~wiv{f`HnBViu8tTS>xE&eBXKaUf35<2Or7d&p zd=$Uka;H1OEmix8ky*n^L`T9BhxyG~vG73%9N0(M&stot%)7h ztc9g?l)ISvVDr*Nbe*(cHJ&oZZpJg;7~tZlE(t#nXECc4o#RbiEQ#9|aJ2}| zhk1`Iz^#U2YT3c_;ybHt92JbM<~lGbp8K=V9WDXyYtJ6@`~y|JS!B=t^pjw1dJl3514V zKvyJ0hlWxLEIWT$Rs@_G->TbTCwbSutI(JU+OA)XYNuONEC!gB%!Wi=F22+j7oDv{ zo&p!=wT6gdf<%SImX-`4-ab;4E#zqRY-^qO|1$#k-duUefyIl^gMM8O`mjhKTxHkGP z6Hb2duWEl07x6xXSy}UVSP!BS0lSIxMH+?09!8NGIVLtT`??fbA9frObM7pUowDE5 zzs3+nCjLeMD9~8ScTN@Oon$@U$uHVDF5~2&$}2HG-H62C!yl!bn&D(Azr|$=_$s4Y zoHEkMdQB}d+d)h}-vh%^8$VvuoRBB>iA|X9+FO-I=7!L$VJIVU<8Mkn=0gGC!Lv11 zQJOPUE)%$s+EWDPhX+!3DZE2~iF|3}5x}CqXw!sx$CK{`uB8320w}4u3y&-Injz4G zc=A4X+J!|)piA@sl4~-Dv(9ef*;7*s)ytoJ$iAop?15Ou8_|mCx42Q`K#+}fP2tMl z9uE9o7dL+_TU#d}+vj^7E}?a?PU3P&+f`#bdVVq5qm!GY~fyWuN8d??YtgmDkmqq5v<}i+p`XH)2v%8QGYlg zw*odzqe_saDOs)n?P_x8QH#&>rTJC^f*}p0ePkNe7w7ht)+E(&RU8+DS--m_yKJOX3;Jxm{JE9jaN#_awqc!g5{(vq-N6CaN68dK z;__29oTZslx|7To9vg$dh<)N`O|8dUDVlC%y|Z$U27h#!xlOzr=}U4ibOQN$-7uL& zua>Quf~(6b(u|zP!!lT$43<(I^@3!utYyfUNL@>ObM7HZYwo4Rc*xS&E&cem3VcP9 zdhBVpCiR(Tai6nX4?CAWQx#O!O|a#+aiS8uypM2_YU0_>#hhH0_O-nFg^REY20`*l zAmQA5)NqF_W4QFD$OOv6AyVBOupK7|*e@(# zgC{TsyOc~z*DgI*XGnc%#%euZG|U+}sokfQCTT?_IS*9mw1<$X@%t>_ ze`S7?e#9{=2EZ-0cs?EwUW!9Tep|;JmP91VO~8!qLHiM~sY6;-=`9QP@WP?uC;UD> z)8d%!msmuDPC>0{5Z!hr4w}zeL50sQijj&G-Pwp?F_rbugQI9Wv^*EmbmlqM3v8?I z32dSA(dG(Dns5y7_-VC-KVm4}O80hIt?(=*`=mU@d;R!RP$l;) z+pTfh%G)N$nOo=*KbCbY9~?VhQsnRsn$G&OygfTRd_32@i_olD8TCOeA%+nnP6xeF z_Yc3h@_EV154m9sjr5nES`+L~woO&9smhL{zU@rjDP^tmQo9i%2K9%_5aHQU#S5of z9_)tr*Rv0?H)IwHvG`IOr^uJhocSncvRrzYuDhbm#tr&HCLegiU2W;J#jo+Li@~E$}xG38hnTq95SEjakS6NO9ZQ0h@ zd3`rG3)uMkZ|f=isFNn*Vp+ni&wynZxW9Eqn*?Q6pyeQ$MRDsTcOSmdW+0Uqbsz8` zox=WZV9ln#s9`z0m%}eu6y+>zbw65^N4yk5)jzp>j0xa2)jnci4xY*cqydXhm&K*G z6zRSn_MtpCih=K+iFJrS<*a)@Rv#>OklPQz?odKbf4fDtCtO1_b2Gn%9|s=PJ6%wE z#%*Z;i)OkW5zyHIp6#SQ-YIG3BPACSfCn>Hj|k@K2ts&|6NmK<$mIun$pH!KdQAKa(*SIZ_?jR>T~FckZ6L&{eN)izr_4kpDgSw|1D9@ z|Bxux|ARzlw56hGCXjny>&hPm)1374ktOLS7~2{M-x%La;|%YxOz9tA@d5OTm9#C= z-??L~Du4oxg^;gq_ zf!>OxfgW&J#N#`Mqp>PK76SfF`?t@uTYY+cJ>NHbSq)~{h0YDQ)Ta{#HQFdxl-O?W z>WVL+J`2zoZowkgbtw}(O9Bc}FTO1*Zk{S&vKw#r5V3d@BjiV&D7aKHHyH@G5%pFzM2L!p2Jf zrFLUaJM1J3$7QSqzk{b={$YBlmq+wIIjFUyQg?@Xy3B2W<7P>zTh^QJzFynaQ zU?^gOc$wVFYX<&Mg*#%3xY#fDN#bo+?w)UWiZ^zx;aqf zZBQVLfzt1y2nOI^dz*g=L2|JmI%KKh>)|a#B1qSjZPR;GZCwmLY`n%y0g4N;<|W$h z3h4FEld~RQ9qb6x-Ag8w3{XGhp;$m6=$!lb!1|xF3uxdLPsv7;8=XeN zysIKfIMqj68X=%0(xX(PVHl-CJ zl&ixxzC9wZ#+D#Q=x)Iu3P9#^q5<_bt&a!qaeUpIM4GT3aa{}>C!_ddhh)3Ap-~C# zQ6!SB6zg#m=$mD{WeH6YbDUW<8{O1{RiARBmYn?}7WAwY$6%v-g{Tu_YGW$m4IBp> zK+91-&Na!L!xk{lXq@B?pk`#&sxuVKxi1E=Eky_}=-1VvT^PMe+?puVrkPD8Ab#!X z2^)y~Ywl~zEXqz~boNQ<)+fS&J*-kq>7m#e43=sO$;L}8v(N4bvoaDE#5Lk)LC)8| zLxjliOpa-kE?TR%ac(Imr#r}I(HN;ZK9yI2&!;Hu5YDMC*VAS7YxGR}L(@yQKNmeb z8C5W}wnG~)A|~8nqP%&JkJs3s4H@$N*RuMH&wLqbv+V?<-R|1)PMq=;ro!e6Z$F^N zH`Z5Tgwd=;>VbyzN(hiem=##%@QI1K29OAH#R)GYpx)rm@p$RlP@40tRbm1~uDykb zxUGA>q4^~mlUqYM0?1jCEz(8xAy~{i$|p|7K_?@HXyzT$Kct(of%4*6ox}xZY$_G( zx#Jr|rb=WGydK;&0YP6nPa;`#zvMqLk`cy&Fd}gk1`OUqP2h~wY~Kz`2y)DNNJZU0 zU`O}ReqqyOciaY`hMY~loS;mpkLe}3UW*m~3iP$*Ks$5lw7~p-ALjBcqJM1r@CM5Yc(i@lQnC;Wh2l8E1e~gA-!n`wzf9n z1iQ`id0uU(o5lCV_#C4U*d?IvV@GwB>6vuBjVb%3nr}SSoYJ2Xh_)5*!1P$oW_6Jo z=>??mfDJ;N9#R(ih!c2{p}Sb%REq8@I)<+Y?Yg`W9cSB>bkc0iH0e5WtvdhJQ**nj zb0vdxe`z3-PT;*p8wBBZtV391(tTh>>z+Q83L0^?2G9ucJ~S)u^dXN3UoJ+S8R85GvYo$U!w1UdR$w zY?m6*KT%2J4r{3!Qe~V(7}338j*Ykq6FOaQRm^~BX!vmRkr?kQa?aUqfGnrCSi^pc z^P(=;>i`=dIYuo@fB*wTVD-hEF)9c`TBCx%Q)`YEDuHix$uSptSzGw$6z557u9q$7 zKOL_=SK#12H0WL=YsY6Z&0ATUTK<0es5lsth%ahJ!Ak}~Nk_`5$>DydmbZwaKh9pu zZgS4C*FXE9EwY1*XI|&yj}Qm=C>FsxycCr)iNu$>33=EzY0gr_^SaVUnebkdyYL|dGK4UiG^S7f{Tvyb5SFiF(cTPJBZdr8_qmUi#w$$4ZZF<9#8C*kn^=xN zWoLob0l2w;V-JZtQeoEZS=!Jxu`~0HEWiivt)GAbGJ-W&E>QDGr9$D^wYTZ&@qt+a z)a&fE)2(c|X#&EaKMVs>po(A?hfUA17CSg2eS9&ziHyj%}AMG|MH)T8| zVUSsm7fbudr81jiQ|DHcxG=}l#iS{V{b=0K{A3Lxofx;mDen6jVXWq6{Qx zT0@GnEtJ;mYi&&&F>Vc*JGErkpdywg^4ZD;rp-K@u;SrWv49z5dIWaJQCH~HGRGtM z6RLeEO{H)%GyfSBYYyjDVcAhN6Wr(AIoBFiP=tr!Yraef>S!-(hX@?DLLK7u!0$PA z;H2Uc>QX1Go1;ZN_W4<*Objb_{NwQ204Fs6M|h=76NAe3+|jye+gPdYH_5vo+9;jH z-{xP_?bRPrLdjFea-3C}J*w#(UbVj(NelH= z_^VZemU>XC(|yeKeivORN;nk0CbX~5{zL@=vSWgY8B-RTteBrsr8c$<0{mK!p)7#^ zU?YLI3?Dq!ITG)(U}*O{$iN1zbiC=dG~Cu9T;eqNTQG%9NtE2HE+v6xkqP0Jorl}z z+SM!=zr%BQd@MGt2LQpw%WuM!=7WUW4*TT%;`2}!{?l0A0cYfd-%-?Ppb`k>DRgPJ zK)|v1qZs{auZPy*9G_P0rc*a7#vnno=+t<>M@kwmIEI|iV6}0j3e!gd)}CcSayScD zv-K9lOAv3t5&*Zycn(xde_bBBP4bM=Ccdu1V||Wq%|dUxKYs(%&pvhvmy^z5tBA7y zS_e!guf<=v2uF>=nB4uhocKO@ZVzPoo{V47f<|KE?D(h!NuU#5iNSGCXRy*ixhJHh z`${M(tE7w0c^$S>1Do;s5yd}-qz-Yyw-zkwRr@HXoZ3H!;k}q>H~!B>6w{}m#MUv> zoH~fHgGdLELeKWRpc!9O`|z*TH6EQd2Z9AD=4VZf**#K$4V3uUp0|!_)7|kq-RVY= zC=Ef6!^|<3hX`%6?LBe+G`-TKj#^&EVZbBV6nfo4a>vV2=Nt1-VBef4xBs|dqW>R- zdxC;<6(6CViPo}tY4dE3-HY|>a>uFp)#t7cad>?}vRf~{?QFv4LNb%&#$W48sVg

FSB3-5w;?e@Zpx8xXkxK&6NNjaO^Q?A@g zfRZ8Vjgp}NeZ!D;|5ULn3K@yp36~`NDL}7c`H6Y=&{|;93j4v|YUkXa2QyzE%MTx= zrPrifU*G%G-RkGb#jM`GKPggMy+2+bG_q-v=u;}B$>Do z75|*I7+oY^{IX_D*bT*w8w~Nx)4zCSp7>6o-K#pIv2Zg}$>?~pph=OssVnYi-%%UH ztZefzvRFy0_0#MP@`-Ebwjrslp8D;Z{sU`7B=kQ;a{n6VzbM4a%JKh)a!gGBVIih} zL&N_qA{^0~sCpVj?(6FlKJ8cb)HlFNzXtbqdJsY&Y8wVdg}jRT`|YgytVGwsTI3+Z zKh%vawR2H>tZt<}?fCJ0cq%1hgh4ICzNzWK15R@K=jdrlK;Y*i>|B8GymmpUtgJbl z=$q;7G0o3MgY9#In-3SC;?m55qQca0Y5qJ+z*~}R!^dKA{0I10wx>oXUDxl^OhbcJ zskBe2gyC)tyzI};n%&(8qX!n@!P7P25@FvF>46K=#3a?*gQb)h4+s|j)EkD5?fA82DHX+=VN~ypdcf#(;g$ zl*dmp#s0nW;prMzZm~fc!#lG#^_xr(F)dW@Vo)ew_}U660n@eVCXBb6^{%$3=(#Vh zmx}TxDT)iOq4Cmz3C1-!CiYMy{-(?*&Lp|=f z-d-_?-AWUHD9X8B$GfonMz8C3cjSHnCc!f!Jc~&sH7{m5_OPgeJD{;4i1RdU zYTUO&@k}=M4hP2f>8T6(qD+VeEHcecfACvWwwLaNMS|_XBympCQmVejH#f7-Ji)CZ z=iC`s1OoofFOm*RmktErCpff@!G%25qgrG>QIhj<)C2>nq`Z4hLx`#~lYE=^w6>R& z30A8Uk~#ZL@#B95pI5zb_s7*yd?xS%>uo6o3OHB;7z1u!hdY!V#LjZXz=9H?tD&U~ zqQJD4HLUKK4t^Cb)qv0lI~r-PKv76xD5Tasi-i~S?Mf{{|A}YR0^i0&hQNzv(|wtT z9SzH+hwFh`pTrF!5w1(0VPZt8X%qGDJgt*g!#kQTR^BQ}dsJCi1hY*_kz-1dDOMTL zqovFW$h%^_gYi&U(T8|J+Lvz#cHyn8>&%|IrI~g-qBGOw7iD4gZ4Gklz@K^8k*aT##43zCa7-3+ z511N4p@rpfPJHw-l{IS|6ZiVTa;zC6+r675bdo%-Gpw|ni@D3=25556LQ=iwY!M(= zh+`1576lE+ky4q9@@U73-wNp|hKKr%Ky1y%17uK~Z7XFdn@JGhBJgn8)E269RM0_#rs$P1HB>*ku+!eU(KJMRcoJ3~j+BWT#aa+UC(bU=hRu zw#7I_;ef4cbpvL_7eykezX=r znyrhAme6v|V6{;pkRtjUt+WirA{xGY?ZS_Vo7cF`Zi%x3oTiEkI)ShPvHG68FTTMX ze0bGzYj(kF?XZ_avXv7%fA5uaC$!56fBiu~pDs72$ba{cf0JM|43EM}=Gw`~edY&j zt03l%R_h3%h#dvhD1HleI^>3^l|;>j%omSC>=4hzvpl4`QA(%Qo_X#`G-UCLsY8!>kuyJ%pnC{oSDoOaDb_OdjOS zffAG0iPOS{h_f^kK}s(yIlWk@_@oMIf^Ls3CBLt0z7{jf831t)fI${5E^&wBiYbYU z5i2Q6yu3Ro1R69~J(5l)!v+dmW6_Y5zYfKwn2f1EB4vqvE%a^0DiiC!-^?<>yHE_Y z-!feVH>k{9p1!%%bTo3`JRg_;`CE)=;>hgNSDKT|k&!Af;erNNQ)emmeT9>}P08WCs6GEJ0bQ>bo86g(Uey+PiJZF0s^ zq1iQ6tGOdn5;LZ8GrmQidRgsBy9kY$t8g#gz3MkB`7qAi8C~+YdP}%#K9HRX%gnF& zzXxC&3oNpXa#wuL9Ry_2{ZD&}Q%~cpB+jR+#GcYc>@l zJ~iU{o_my!|AmlcHQu9N+3MeOfsxd>NwIkaNEZ%2OGI=KQOLU{wNLzB>}>6%?R9J& z`7*Hf)(T#Qj$%BVzg@Mq#vmr}PdasJ-yhVX(Ov6}X z8(cFGD}{1A!q{d3aidC8L(Q+jRYcG9BtR(2#8R)4T2xif;({^367!CRhknTTs0LeeKplgFvUBF1bNbOi#bsyY~pQv8u1Akn* zRk4XDtvO>7c?6%VcB5NE*}pSiuN+g^GWDTb1){W^pDEud4bJ9EGVGo63AVAgRMNax zR7*@EC+mXF6j#%VR{=fB4M{qR)ak9g8meDh9)5^+EI4u@{<>*nu-`Mm`cG!!Xbb-- zmUeqZ5(Z459kgB57cpX5kZW=x)GAF+{pC~qhN0_%#}W1||G}dNBHy#(O58Lul3#87 z_>6m#zkBg>|C!%9ji$z6t4lpMuecQyTWcQ#A-}aTuGjDY>7-s+pF- zqIo+kr+y`ZA*{MSVK&Y~WfO&W0vu!&arr*!$)%s-s89Ny3Zf6okTS`%Fc_+sW80rb zT+waB0fPj$6!f|&f_b(=CB8?vl%0PGMFFRUhUvQtuxNzAPNd&?Ix@Y$76aRPD9MMNCLT(8gexJRG= z*>c8e(~bk|*B1;K(B@#DSiHz@8mJM`8%Jng%CT3p)fby|p4DuD8xKf}_4QUt4d0im z;q(=a)2eB#Yfn#xB29N~4h(4L zK|)fhLj-E0FmtK9A4OsjZ3MNkr-|Y^PUvr$Oz2sTYxLNf8aY<5L(ff5n;yL*WCBxP zU7njhX(@OAqgPbTOfo3Ap;YqHCyX6`gP5OZYPtE@5ye9H9RkfF4DDFGXT?=P|F}i? z%tHz1t&3+$8@B29kzg%1rAn*BN_6xg^lEe8j5>TeeB=G!WgkYdi##j?ObN|QZm_5?8=N@bo*+VN9-493gcSc1%NVl@y?z+u@uxm$hL46TEnlf}& zf6%Ha6|HSW7Ds*G#Z7hBDppq86nM~if-Qz<#P85FzN^Db%@fX-So(Z>yAGkX5ps7d zJmPh}LsZsr8vU!=U9Pq!?3i6=baoD2rY0}{u@9d`J6GPCogtwqWnc}p#EZg9l$Y0; zXLLVj*nTJLuBC^zkRt|5>pQ1@Id3lR*Jl;OBKJCk{v|Aaa0v}_QTdd#vQ(EImzXLB z;6}YWb;sy(si)V(9XAEij7^teVYe~^q_$u&|W-ed7~#_$QcT3sD<~hQEBC`k8VHcE|hqpb6s2n7kq=Ym&b^;GQX& zKXJYD`9t>@s`;t#(qN`1cYH7VrzgSXvjN`G5p+U;>wP;kW;wqf)CW1aJ~^Wg+r*_f zLN$`#HmvUC?-ZN~`ImEk@_qy7{ZIbh?ZLM&rt{~7vSAdV>)vmEAmbF-ev<;|-wZtF zt@rVc?R9Wcqy4l5tq@zb*Rme`{I)HzK#-O~hWc-t+4hGzBQOGxX;N=z+eS907A-b&w$#9D&y}{Cw9} zwxg{iYh}Jh122q#ecqyE(Lh7fkEr`H|t zd38$2y62J|lfA)(vB z;`%qh1Zf5LMfms9JiF$!xKd?Kq4pPew~4JxK--8P<`#dkwj> z>8xlbNKQm6@bvrdq!Q`zSz4-%MKGKw2hiS8ip+=n0J)2QgO_j}Z|8xEH$)SdPBTvr zq-UN)IL`SzD?b(zZw5b*Y(o7L_zb1lN>N2wyeM{qXu9wL+rdiiL0J1iA53i` zLGA`xN~vUDo)Qeko(Pr2QfZ0EOMm$j${8t%t~nOvfn2yaaXP&YDU?)kcr7-_tcr9C zas>)-&r$PqP`&b}=vGaPgGI_^_wg4Dq|+WK(>h)p`C?^WcYds9m0^={Cg(51DIFKzsyAbKo2HZuProiI@0Z6 zOYu;H$DS>%pH_vM6$wR z2L65RLZ#rGxhh#UfCK%~>_EY(Fw#@dG9Ot(a45K7q=2UL)U%EIM7U&+Csu;_WCnx+ zx?m4mSCZZY;+^Rbcd+Cp5X^EKJ5X1LR;yj0)_i}q3u(?e|G>LVYX!-@OfYaN^R0|` z6k&nK1xXUN2%(agNGoBg9pHNja-0R%DN={Cjwl8$RD|0cF7X25rz^!J$`w@lY1Unc zRfa^zWW&aE#Ar8Bf;h4>4_#kWix z##B_9QLY_;tQ;R|J&QMLvUYIO1XN@_h2b;5%o7UI3e#m2*gF$y={z7wlVPzA&Pn$n zi*`-RGlj{DvbAAZVHIBFln(XBHjG5CP&-kNY4M|zrdZh(?|jHW!P#a#7eJdbEe8+Y zUv81m1{5n5At9BXDy4K=$*H(pG>Q%_7=@&(eddVMS&m<6s?2^2!SjmTq^&&Ei@_dg z<~Y^8hI<`5FN-S|TY9y_O|+Bo&$kB~wAgJ}C6&I{BH11GZ^ny6k=Oq5s!ABzts z@x7{xAiPwY4Wo$umu#m^5rN}mWH-geLc3(?Yfy5av4pa;FiM+%HDUxlAA3#@Ug$MR zT8jYfszJfpOWh%{v7eIIPAh`+nY5(5mP~>PJJlVY-g9ecNTm|Ia+fu^6Z8=!VnHwG zI_QdGzc9DDxiN$xSW|ehZafw{1PAd`M-JVAp)fKc&|(T@EObD_pPR00Z@>+E<F^o(GGM&ACG3OvV|+RK`3=t4GqmAkt~5 zUVTMKbo!P|Y^v2?g046W&-Tkf;5LOOAYd=xr=Ax1uuo_rxQkUw%!eR|m0JV9 zK`b~eG=X*k#m!oax;!pduE-^aCHQE~5FkJY$OfI~Ax2#nD(BAtWG7bK*$BC;jO0T4 zLw+^&^xAYbendMIn2UA8k?6fCVR%u%v1GLqXv z$kg|75=a;Y;2x}3!8PS{6QR?s%PUB_`ADY(i6vxWps-Qecp+N`#FX;PTmDJ9i*Y8N z;-n?Lt&v9W$7SGh_*fx9Rj_N5SFosLKwef^i`Yw5&!fTJTk22sdN>Dyq30e~bY6L` z6=7>fKxHV3UfilPcohG}uKtV)&edYNxt}=fJZBf$+W;~#QoLH#odXm=c zUCRL(f=-%Pu2$Rn4NPihYjwtynQVOc(6nSM@#vcKpLxB^Uou_6P}1`qjqc+w8qmxf zqJA5Ud(rl^!SDEZ5DFJDp;xx)#+-to;O2sIOFA5%Cn10T@2oP-L<+R4am&>q;(moICUpys5S*a(e7QWP9e*WgMWf&XSYEZKb=6}`w)j`rHRb4piaBmq zzaSq3QlGr7OMhAIV}bkf?_RME&ZoIqztJ}5`COl<;-NYAEhglNvS1N@u`u}NEiU-Q zGp9Z6pkk>97{f~ln598#%OHb;q)VPtoGxD!gw@b4OD0&?&LQ@oO_h=UWf%+ySrwV0 zWxCZ1t(xC1F%dPPhy;Z`3BI+i4V4l`E{dz@_oLjj>{Ne3!0PH-CssABmF5sDrR;95D7|S=m8b3p%cg$T zftI#hd26xolKLivVbY=~R&W@1tio*7R%)GFRVPHd6y5e=#tdU(VWsK2qdZ&L3_uzq z%kCuIP&$%rrA59iaXz#XiwQ1S6EWt{>%ZoO?c2vg5ObMtFpP<`c(BT-v^pEHi0JX( zQe-2dsC3sqbj09lWT~~W;tx46l;$mZ$1%7UGaqeJnq?O=z80YyO|g>TDNw;Ee#cME z@IRn3zHReWvNlrVuH4G_h*B_VAvZ$vMb>jYQjC}>Fje5ae2J=n?&aZ{PQT7!^&;Td zxl&tWn{&?CH4qKXoeEUX{GC^q7#!HhSqpT#k3XjhSm{A;#b`^@CXJHL@&NtA~mr>Kx5Z_r>KB{Wl?6{j+TcY^L^K@Fk! zjDCG8?GQVok-l6{Wd=5$=o$uw@On{fLXoFYhtQ`3(kreURO7PQ$(}XYO-~3>Z-HPEkc@sBJ&>NzSw0^ ziab-i>F9MU3jE^q6t}8bV}|b~V!?g1?AFi+f4U zS^}$C5xGP$+q=rP3PGNg zw>91ez9{D48P&l+p&K?oow?llARgNRJk*7&>b}ECyL<0z-wZR@8n~wXV@?{s%pXLZ z_0)?03FZIayHOTSF4q6ftvUV^<^PuT{%YA7i{xI-@QSrtj2SC}6jSu}E@mx~^L)QPP7|ZMD3;OkexX49 zPR!;n$xqY&`~1*$oBFr6T-jCcn3?)B9^?y-p1$wrb0kDr%UXuQ>hU0jKV@SxubROr zjj*rkn?JAcOrB!z!!IDEX{qyTxzsx#&jcEDc(1S9-Nz9ivyY~v*Zbx4zNE{bmGkv* zJl5BD-oW_`Wi6;@)bf zHUzdeVXmyz0ISZZp1bU|jqVwZ@HCX8--5rYj5$#t4)XWy;d|ps=T z%^fO-I%#e~!m7^66-KesO>!G}hGu-b85&+CcD8m3#6-U*(pbz9+@h#hn{06Z7bC%^ z4IA3U<_znG!MIp<9B-0E_fkYhfVO$HenMIQ7?yn$=cCdmZkHsGN^3)$%Afj{!I+~5 zHvdqxDw?42GiEo7_1_EQ4sK%bf`rwR(958HB{Sf#Su7*6Ei)Yl{gT+bbI4>C3h>ie z3xC*o8Qlf?3h(<4PHgz%5b>UYi)_!FXsz$nB$U0ZKKom(?NTE7)Ro$S;wNqRTXd?c ztmnawL>7?VY?Vt`5ER|A)kk;x^+SX`UMD1+U;yJUXcQEL+cJxJ3N%`T92FhGo7<|p zyZiZ58JCioNVfLlH_Z1~Ha z>)4^D<`&l-?^hBGho4B9BtGyyC!4v3Keq4MHk0hF2>#WGHZqqt>VlzG8L)sphh-b4 zMo0sZ9_!B?)p+&#Gl*Ds;>EFYdEZco($v4IKHK?fPxp(wh>nPr>7_^n+&kK>i}ut{u*R@<3r={PCFs< zI%zbSoH4Zk)`QP|QD-agjPVer{*tiCd10E7IfPbmMEm*FZW_m2gJ6POXwlQz*a6P4 zmnCf~wDd_=f_c)yAsF0tV9Q0g>20xvqSx+1u04-2%s+?ByM9aJy-)+#_4f zU*JhNODYpCWy{<00!9z5JGXjR;hOsslH9?Q+t`b#^iF|BYvNr5syF4aiiGsP#i~o8 zkl!f5`MZ?Flf52g3&kKQ-k>n?~}6J1&^1b}$;4mecc>OyVwQ3ng2{`*AywIIo`iBM*ed8`yc!sC>OR$y z1c-20=O3!?g1Q_6hlQjTE!lD)5j_fUwJ z^B8PyR(kcO5XARM+Nu9kJ@6*5w>k*Uayd5e zXV~l9K-GYgubPW%G+%F3=EalFy11$B)TVY{4J!?)cO;t-6eJxOjL)3ty?c)~d2mJ%xlI*k5S&Se8+&C>{839p93ee`(z<1@B zyC>YpICse>_&HE#n*J+E zRnlGkbXUGfo_r#y0JSBed13pQ8})**f=t5MBfC;_BhrDN=t_6Xqf9ngGgG1AJuzzG z4r;e1h4;IiK+erQZ0xEfq?-sObl@lw5N)`Ki;pTQ*vx3ld3uv%8akh;Hej zrS1bkw6vj}%fN+@ie>)V!J*OO`Dv)Mh-1Wb+q+M|0#2`4#mZCG-8xnA3=p~O42^Y%{QE7%Kd}m!R33w8E|RK#|lGP=W6lr1T7U|MYJ&p zz15OnMW@tl;SsD6`FG|AJfBHx2t&)Z6wyUGD!M_9S6$tTqvttQY65*JkEOnCO=lk* zyD0Dl=Qb>ekFJ#JW1GAl6R=8^Z+UQ6?dd{U5BnTA&Rz-d?O&2L=1|(41!{1}lksUxP^zeGUf_A`GMU@e-a@TH z<^(yLBja0eD3QnK<%mb7uqvnY`5&T*y|L{-1Z*hZ zlg!o|0io|{KvpDH_(KaFq-I1IiXljJhiL~`UQ4No)`#M$11n~EJ+xSs)p^^3;hZ0x z7xPbRdC3@7>$*n3emcV4%;fe3xR* zh_ID}%$43@D;F>-)K-T3N9DR3=$hvmW~R8sc&xOzJ$>GU@+XK#xk%+d;%|vmP*wEG zl)H)*t!JKT8Ni;hY+kFM=(fBD@bmjuOf6TFDMv|Pr@y5aTfJ&_BSme&gLF!6(k?Te z)mg9c#+BP*&~tIU5`YugaVJvsSo{+gWYDeWiXd|Xu(}c_MsB>EHKu(3w81LB&$Ug0 zrDp0TVVJAhI+Vi*C>L3iL1Eqmt2EeXgR~t3f;yIhfJjX1bBiy^l?Fe%)X>lIuglXY zM1{m+=FAtVhq@LHj4b>z1gbm?!< z-};Q+7Us4`^o3jN%A;=1xol%AP z4wAT8C+Eh@8esBlXugJ)uPoK|ONOXqA}BF07FWU<7u7~zy0W0=Zo|%J8A7+qSVk-v z^}Zty=d)$u!MEYs>!CiI(@~R|Gu3k?wQqn;4P}Gk9+0+jG8n-{3tN(dnKK8JVJ%=z zJbu{@`+H2044$R=U8otwe>OULRBhd2;qz=-VS}N!vll=R{aJafOPGbz^U5-tR2uny zfAx4sR{mDccQBC-05!HH#Qp7YoU-)zTFx~^2JMl9HtWpudl5l8C)Y#leG?IW5-S>w z$N0XZhW10@JkGQ$PZ<;_7jN&IKWoindnWq!efh!{ELe=^6mYCfQ+;6V{V9EqoXLeh zvB(Vj<$CsPD>KRtCP#vs!*`u|;XKCK`exV?xZiq1Tf@#p?AW#R&U${`O=HYD^P9E4 zQ1-p5ZSg7wt&*TQ>&Dda8$-&PKSq9G@FWok3F=c~Kq(1yo?OSJrVqh$VbK`!f~Y?X7g{ zcIONb*!1_WhZNG6rBQ>kb}@y-KYj|N4ls7tZ{}Io&ocRXBAZ zQ-P#W2cG=p9Zpr)s)QZ%Beg;==AAmJi2X0$UD&a5T<2X6wczu^ed@S6QE!Zf5)3Om zBitRlxj1uozgP$tkg@3-ozK;Yh&ha03u%xC_=Y74%o~5f-|M2U87b2fdf(9#r$;R! zG&^*jZ)YI5tCT&$MRz}zpU`M9seWHG_bcP;#<5b@$sgIzR8tgE%M;PcdTOMP!bqs6 zcy7jCkm8da%A1NGK0!cXKdV4sC9Dvbg(H4A^m!mqPYir5J5&iXYLSno1$BoHYNjCH zX2g7PKUs%L$CMhTwY3|;o{!ox1V!%{O?nZVa1%ej@RPPE{?owu-`iZ7*y$PnYv9aC z|9=de8UOKs|Gx&#G4dUjzzoorFI384%L&F>RzVD#I9?*{Kthr6v~B#6{Iz2@V)xfq ziL_Jh+|is^p5S*ffBk0B`;Hmp-B=8CMwAPy1L|EGug*ALS(=Po{?sjbcKC zyRcEXmlIa7O|w9-eTiGV^P^rdOp#w8L!#*G9DW zyQrhA)!KULOmyS(|AR39df)%D88H0!ga+fkiCO=R>SFxIc<`Te`QI!r27D$)M)v;+ z?z&RlvLjMM;akyLYu}Lhou#ALhPRSq(?N1DDA7(uSW=UfC*4*(N zebMkID>;=Az=lR00-`BpJ*-j?5Lkq5FMv=tHI7<+&~X7(T^api@2|-d&)6;zn1F z_xV7s_j&RwXNTwH^P0oS=gPFmWKO{|o1L~wc}qL*<0}ge31qwfGj2Rp1H0RsK@wTIpH~euq1%B5_+z48F3oC+ zN^LC+VD+Y-SJ4HsR2J=5(9Q^rGE?26+&kVNU?3oC%T%Vz)sf$1?>9`TUNx8PpE%ua z31?^7=@l&1F2CKpn}OPX+`%Dn8axyPD2k7tx|`XSGZ)Xi`A6I%i|}0H9MMxkngmga z%zW~%K;Gh=`MT4NM;OnbZb`d^4&qa<07YJA^kdho!JPDySHcp zQ`Y;yMzePj^Imt#y4JM&PozFs=;smOg*UvRxq$TBvp*U{B}YXJdn;65+Qf{a%Eb2! z2(u>U%2Bo$d7`x^M7K=eSZ~yLbA^QL`r#EZotdL233TNE0OMC3+|HO*rruY$ywQ)27m zc=-1V?Wb;TO!(J}mTq@HFIdBoR0V+Wc59`Tq+-&X+HA#Z+TVyE&QyZV&UgL}F zoi!93GNfn_?uEP(;lU&@gV;)va#zn{MF`y+qFC_B!CrqXeX$heqtNE-&MN#O%IW05%-v#}8<5YJ@^^r{slSmDU^5 z3RZ3qxeUWZEA$IxKf#F93=Dmye5VC~6&=pH42L{c=2q%mtO$L*jS$Yf2`sMR3&sY9 zVGf}Jw}GuYp89eZ#k9SJbge^&wND0iiFN4OePfFvK^xJb!*1<>TX`|oVS#>_J9i(y zEH>!Q@{8Tt1>=6qyu&1X8`;)B%p2JUGCw#{Lz66>-_=ArM8CyTwdMbnJlMkJv6|>fzrf%4szqc{^h$gAmB;< z+8idd>*rc}3yc>rGU8rq^6(~Eu-BSpyHMH2ZG#3nFtJc!9Z80!#L->fI;kYe*)qw+ zFfia$Pf45#FoTXVX=qnSw$o z)I1ic(ktD{;FjvpBBzmN=j~4DMQS$cU6-<@-mc#Om#Ve9rZ(!$(>0P6n~QBa2>^7B z)GZng{4JM-OZHH*K-PT~cC4JXFjdL|Czz@V?aHE}p)=>3jZ!KVs-Qi!gq72&CTZ1O zAtP@dBLQ%E&X_J`29go=&~aohQCJ5P$0QRXYcsVPQ7#kHHp|P|mdlh)m8J*&?EGl; z{KWeMcL@S(4_^?RL4Wokuz`q>N4%iP#b|yrOe=vn!8neV$Y{&_E)PvOFq;^j#1FB^ zD5Vh5(8dlMARNY?1)OCZM#i|EqVQHXL$`~ilaH#LtxsMD%xDDN6kd)VXOEydGr=xH z=kZaC!LI35dY8QEIHRUafhk$^=Yc7q+@9`M#cV2tt)Ql&6>^H?xpc64R#+lI@)#er zt=yKbnhQ}y*}*l)f`z4s?SPRgB-MFlZ#xHFh3$cD2I&Yg>EY~z@*WaOHMB9&*p(WcRZfMm zlG_70Y*_TIAlDfi!%w9oyWa&<7+PN1+N>k&tiv7Q?RXfAsqitXEZg&x z`_O1ALQ9aUJdrnpi3ZB4u9Uy_L!xwhXvK2tyU;fi(XR0Y-Wllf@ihaMb$x1oBYfJr ziaQ0nJYE6$K%I&t!Z6BesCv7ic!QIA+?_GfL)}gJ40`iQy zJZuc@tRep)X_`6VGyhHets^LC=dMM=z{Y`3!}Qlv$Ux7|h|j^uszWE?Y+!9+Bw%Z1 zZGuk^NhjcBWMb=#&&t6BN%zlv{>jiV{(T}kAp?7H6ALqQ=YQ%!(kVHc*r?+F^(6kQ z`M>Z^6Gr@h1q_Oif4l#G12!4|4LkWCAp|4C|AY0Z6E7e)hz}of^BKu;jmz4YokeOo zMapxES;gny-AbnQwvrVBY!_9y1eiOuYYW1?JF!~-+bipuyKFUPvU=l{Zi!8nb z>TUlfZ{qexMiOenPHMQVh=Da`zHXl;gN>c%m3>#^jLYkgA1QmguItoS<_yWK7&^sL z>y~|-M~;xLeB>ewaHu^7# z>6Bayo&V8W+0n)1U!{TuPA2~d`5%?GaCCAOGB%FE$zW0V^f)@`tu@b3i!ubt(Gb=ZL{0Q=d)QC~fnNkq`^)0YtUy~N z_=7zX!&)2tHEAt=T2NO#M1!&R2}jBSDyidK!BU47<^TGgx_0~v>s zxXJs++(BJJiVO2hp|BH^n7KJgS!p*|3i9mtrG*M?XmPkojIP-~sgZX`q^h!lKY6~Z ze=aqg*|cJ-%T>K$U+IhDL1+ZZPY^Ahw9hLf7DF`ptAEne$r0;>PFWh^u0-twuXruY zY1+Yff@Go7eX(-{W{O}=$vu&6go%7zH;C$I92=T(mOiC@P5|8;PHFSUfps-&xlEY zJ)wB|V+LuKo1Ho~!pf>ksR+JM`14@!#GK@B@DF7Ef&y+D2JkAGLz=~OMq5OE!ZT3Q zxez|jwflA8V$P&LAAHU4G|O!9iVvb0$U1M!J{5Ip(qQLvqq(Nrp28_f(T*powZ{d~ z{Zx)yC)_cMTcEHSfje~6ifx0?eenjSIh?AL6kmG6_)%r0SA?n@aZJGX{xPGZ)PF-g zuP1f(7Ws~FhK{8OaRanc495e8(GC{GytU?}QjStOfX!oMlxG0D5qS~P}^_d|JM zmMALf;nA@%+|0PRoD|MJ&ZPqvwP=uTnMzeMWLva}Ib;zvZLHPAfitDqV_4_Ww*1g7 zb?xSyIxBb8=0#nM69FwO9qE*o0)eZCkV9>1R9PR@VqFL8^uS~&mg#*&hWC!N^=_mB z1Hj(V{>gZ!Hd-!E6XrSAM5h4lcZqr)@p1-pLqSOB2oRNUHF&eO$7WA!Ls2KH{a}ve z68lYSy`?+qxpGB8WvC|iz{Yw6)3APnph2ii8d=OEn3TACiuBI&q{_%p^;D;z*N=L;-c4uP>brw8GQ9s{sb52@?cEH zUBUM6*I5%km)@~~J8C-(5ALK(P6tiA?1&YCefBKNTSuH7LiX5~2`L~iLB%-grCS=p zC3Y6t*fJHb(UBuCO*iM)uI!$oW}?NV1>Ttnj82>!Oc^mNj(9APN`t7{T(#wn{co7Q z)`Dh)I7msc7r88SHn|)noHE5PZ6k2cUA>4Er=$s^%PgeVHUnT|fc;+~Jh+0PPdeP0 zU@`6=i|4{wLJ!(t2zf~u(MnCmhr&1EO_9-!FR_m_bx(kHWU_VF;Y<|=7hOmEcTWZG zAKdY^Ai)f}LfS7m69-rH7;S~w`Rr*x)x#@n4i2m^;lNezbidjNq&T4sg_pGu&9 z6=TR~C!jSGd?b5KSe5uiVd2yRfCbl%N`v=DpR? z8a&&MP%=~7Atp#f$KPP6IAXItsBtA}1^|erQbf^aIVudIpZkTAjYqjXZ|FXJQ~^m2 zir=~_1>Dk@UNua+$b~f<4tF#@U^34=k6TY6HE5wNa%0OlAM7)%(#kC$x6i~K6=)y} z2u+0?T#nKuq+hDf4{*;>D|kuR=nP+QQrTr-H@h)9^v2J^fkhA}ZW4h`I7Om6g+6;O zMvaej-)nCd1kZFm1xzfy23o4qZ-`XY%%*z4_$jeu}$$Jj2c z#gDuW)ttuUFBt{-*NgrjoPi~lAk%qp!PU7?6*aV`FL&)qLsTzqPMFJHst6$+#?*ug3n=;|R3Q1~dbLkgQV zrkaX^`lRsvxsgVxFGj z{=*w%%;*XjP&a9LdgQ$$&oF|;b{}KaoE3J$^&c%Cqk6g>Q@PH;?Lm5zX{-skH7YN8 zQ5f57ZJr5f6X8l!w#zXrEzStfyMzu|?yWCip06eYDo3p23`t{=B8j$*%=-0K+l z?*bK`wDddw7R)sTP& zc>bnG{C`;B$Es$*c_D~0q7^I6B}FV2B_;Jp222=t0YNq2yq!)`waJhQ5cK!%4DnnK z5Tkt-H}D{pw)-&;!0kAjvkgRD9zb-WP?g9e!lSN(nBMoIQIms9Z#7_p8yVXctW&Kl z)P4hj1j2@m>-+z~gr?+#@5JvQELsO>mIg6HCgL&D+4kS=ClrKU_bp;Eo(Jzh10W+#=%GYplWYNAtnB)Zo zl|MuHg7V%9uu0?d(PU?;iQJRB304XuNN}Y;fTc&;*~)c zvb(CabII#G(@e)-;f4^klT6&4!j-hOb*ZT4D;% zBE%M_ydE401=|4wAqhVk>|@K#ms|Jwh@_d zsh!tzH%e+G1Xm=L^>y@E)F3gk4~VBF$hO9u{B~i|*r8+9R4r3hg@wQ$fts{{(9~pT zdSidXyeCPi7laOxxBIK;rl#)*HrTf888hU%j(*jotzVJ1@tdZpvzu5Il^q*w48!1tO)lKNOxrLCF=mEzs#NG_h@I4X2IJ)bT^GzyOXBm`J7Yq2zyJLycBS`(H05&lJ;HOkAk5Q) zG2f7B5+{sJp?{gdDg-cE(svS^V;jWm0h}U!pGa9Ur_qofPXxAh|UAyI6IvhWYSQGjJ(UVNShHNprx`+4BiBI?Z4+m?3rUh4PjV#8zG( z#83vJ2PhT{7s#90fCV+D9U905Q__0J z)E@`*qZ;^hs8g$EjvpHY8F07qOn^mk6i6Thmw|Z@oS8g{cnYYP%3y&%8_Hi8B=-?8 zFXbIf*VhK!SJfTI@9Q`#i`Ax3iS_GyFe12Xi&~k9Lf?y9->&RF16sxm9c&{-vSltO z>ttR_5lEn8D1QPHScQ`c5SX1bt>7<05}$bt0c2w3<&?{tj!^wK@7plojmr#}C2yg9 zG2G%dA(K>WkThmc#hceOsT;;k>ysDd%@EfPyZ8Gg&J0W$%H+duIBvMD4%>`64&|nK zI%T(NXW8sXJ6;YyfKq~JCj%EHa+5!$_B%~%ykF72FHd}UJ@p6t`N#JCqMg9X&VjNV z^00Png5@FDl3`3^<_*6HXy7eK0Bf+Ul?0VO#MCH~x75f(Rh)g4oZo7Ne*2^sQna7x z(Onk7*6DoR7j0olKo6Os$Z))zk6mNe!>M{Aep&5OG{E8NlBFp$Mi0h-kC79;)Q6iDs!OR<%pmSU&h-se%MR)^IbZiTHLmToqk?U{#@m@JRqPy;wufu3=nr2nLfo&* z=`uW=AOCPQtktFb(J*6OAZ?&IGxc={NL9u8H~?30l#SECpq1^WdbFPb66fNg6Q^Ma zD;xNf=zXv?lc0Fnvb3`nTXwr9$tS{Nz7dW$aq;nvJNzC zM?yyF?Bs5mo3Ro*oB^ve-V`x~FyjvljTbVhYd`&gOt6*BH$x?u(e@z9Yz{jx-*d5t zZ9yMuBsRa#=a0OfyGO3@A-|$@3A`dWSEe%=dQG7?frS1!HxqGaCejiWBGyuz${kp;oDAO0jXkCnCd^h_%~znp_2WN#SX*U9Yd>``R^JRv1a`2$_LArl!&B zsV=3)X>sWa!ctVD)!aU=>mIYhR@Gc$+zU}_8R4nfgNq_Nb|AHry=z2em+Ln>kd6o8 zsw<3$xN=#9V9LCm-OUlHlaAmm0DY8=nIs09!v`8<6}N4w2ffv)mM)lG0Y+(rPOkBL z_1lWphpOKOIzJjSJ0#-HH{C6@O#Sqzx@|dly)==|gKzIf!u8I*?)-3;YUxe&b5}h5 zz6T)g^HskuVetl-w@gtlDsYiRl3|h@wq1}Xtt4e13Vtd@R2p>NbRHVCG*5zPs8)Ea zhQPw#6Y2bcQJr?)S~sW*FDkc4$7N7{EPJ%-2TF@O==nUzQo|e2>PFi}sx=%r4y#DK zURk?ef#Y#!t@!YG-vlvG=SoN+!l$wg56kXqM&-gDCr+EQ1kti;vZ#TCpUG=%T1}S+ zmS8B~_TLOgo9-|>5~7nDcAZp`g5w2>*l5g=I_Ri4Z9+qQZg#Of(KvU5@aO`vSBJj0 z6f)=@G^gjsKj+7c*$Qw5#VJv1Pf1Q)(y$B^Es7QRt&KrfgfMz#tD)_x;HpBpFw2;s zD6s;ZV(QG}(<&6*QozVYKw7%(4vSO<%7ApV2Vc%+hAKn>8RyB1>)UR=64WoS2e!vF z9#B@dn)JTc;Gui6kJ-2|+DsL4HE*GEiJ z9d(D>$kz*>`wpX}{fm}Pt0H-aGei$$ z=~OtJUKepR(hFSgca-OgT9q>Mz$gOY#{ypy-`@rPG4p)l*$6TH=xopqG z-{5{|wr!7epP0*rFuqrMY9W#eqTRqV0yS^ zu0oBhR%OLW2I^J6sImYuV2gtDmyJ!E3cJrhg@1ot3&PKl!Onp)=ufdR80=viKW5#E zbK}Nw-OZ(x$obyFqM+%b;)3*esCGS!UanaY zNKzd--<+aBRn1LtlK~1txI5Of18{89#7$7NX|I_XUeLT-R88yl5#GptI{MLpt`zel|6SyJaMEN|rIA2i%BO$&@Mo22H_j*wxp8=ruDqteFP-jI4lL*xFp)E7;qmA{%pLYxNwr`uSZg65aUHg!$q=%#6>f$SQ$mF zn>2E4ziCUK7tHeYK8P~%c{YMvg(EwpxkvEnA1gpYg@P@0g}6PS+=jZ%7=@m}s7FPkc8$Sv>Q7hqs5Qs!x)O z6*XzW8q1%0Cw-Vgr;>R655U_x2 zB(7k%oU?tVhASrIC#dIYyMkgTV8;hwhp5_c;^%?#!SNkT5Z*7Tz7M}?QleKX+LhuEu zw2g}J?iOEp3eJsl(zI%*Fy6}MFml<(K{N<8YQiKOl;EmS6K5`8Wtblrve@BSH|NpA zih%_eK1eckk&v_CF=54sxorjd3`Ir|#dLpP4)1G9ZL=#g)^dL=g`ra9XQKV4VYB3e+vx`Px&Gu5?V~%T||1|M_F9zf_QS zRQhf2qOPYc+SB*-{&tuwZ>!zrrS?kj32jlSKdQf|6?UfA>CSqOVo!q`eSt@uoz9!` z)88fT=QhX~={SPb099`^5TH4Y2PA3OwaE~I-Gtlb9q%aBataeZ(V z$4Po-#R>J)LJPwpWoT36>RefM;k+($f7j1-ZxV2;;}YrA$oIsG6o<9hKRLS79}v!tLc6-pLOQ35c^QJF@}ioIgz zALOzbBZ)NHc>;;dZ&V&d_L!);oW7xc3-*&Zy_?N3bEK0?c&F|^@w9KR#8|_-yiL=@ zDG2uth^>P1CPq((*Bystv@P@zCy?BV`Zgp65wLLmxKt{x;;(S2UEdNzP6LXtC(N{? z1A4O`byN8-c8<9lbt={-ENd}NDp|tVvGJc6_VxMN+tc!zSG-tNjhz_a0#nz~EwFzpZwFe1EHp-k4fs&NCiIJCM*EixY;qeRrUk0J+EmhK)|JLh~_`0CNFrKo2j5 zCHA1GJd0ia)p^l7MK=YS23<)j_?gu{B>MaUh9cr^FTTB&hD{~eX zD>@$wo~t{MA8rwMU)H=>L`k}06Y4DD+%#_ND2`>DDZ&@dmTe1X@}p0rPlQiCmXa{( z6N5Wuy_;A*hdD;rr~OwVU)8lEQe{b85P56HG}soTX^2F_^6~{gPOeT0@3}2-c-a>c zY;_(!C}wK>tjMtkeBDjmA9?Yp+yvfyrfvk=M3a`5vKcx|3ldA=ZiyNCPI>xd1>Ok@Wq(wj5opaMd0_;? z4Ne#H1D=r^M3jQUQ4Vf&bA+A==}jUwd$;xGG;VQ!55hrpwcM!MQ#$T%UmgM~bvh3# zn%m+D&C%;c&N+?PP5Qka+RYpgXdfq-;jz;n^`)uW)yQsTHnB4$Gf=Xqxi>a5qGNmw zb|N6b*7#H$$bQ?#{2aU3;SOfv{B>*B`8ZDKAfbhjeP7+_=}mS%rU~X5#Kd6SY|qV8*OhKEpCN1msT}D|N^#;)@dj zsVgyDoTfPw3%Blc<*u;*e(8pLk>Vsj7#F)I=kVfxW)wo)Kdm zLJDM|pkfa3`)Yv4+jjCo=W6)78@v0p&u&Kd^2-NHR2F6|xW$WMUL0TChq$Qgc`z#E7sw#XmpmQUqu~`E^~u zMkMS&h8?dGK#r44x*snGi8J`=b# z2b96Eek~>5fgEF98>x7TFT;&kP*#t(7#Eio0gN7|zo+L1K>OFimt8eGQ71{~+v|Xw zP1xY0e?1P{u7*2hElMuJv%>N$XQ2vSK!&* zwOk)w|Js-ykfPZt;~lOGx?Sm!V`Km@xRHB9+?0X7poHD#)=e4e9b?L7fSavbw(kM) zwm9DR%p8+h?al=curCb=jL?&J`(~;n83{p}R+*EDDM7~oFH1W0u%YSt9z8LEkSM-6 zYdn=|j-rymY!w(-pkq@^!!u`(^c_SfOf1_in%A!9exy+6{R3<(1T{YHy4jP)#||)~ zspH$oa}O*sZ?8t!b=UL$KFh}PPRqe}$>g^c?;UJ?1!$oYPzs0BcI?Qt&t-#Ght;?H zdCtR4u6IWn*_{_<$4WA&vydppeveqZ!bQ2Z5oQV(q*GD70rK_v+d_dt%V~vjHiz5w zCSbA#{(XDcS@Fzoub~d?GDVX=RhJU9S+XOxu!LKo9HX5a<-lP;&-f3+dQYHcO2mut zUSl^N6v%D3cW3l6Fj)xchc2b+iwG{$jfvG50T8*1&?;30v1igaxt6&7E7MfLlCmOV zQ2DcAKLxy>QUt^exQKH%=N3H4Y=VQDf*9V%K>iQp&+x`pu)0(Hoxpt*`Ry<%k2==| z;DJ}F=jpe7cDyE>e0a`7fcDo&u}r=QutD(zFezgR3bErBi0kSeFe%p3MHFo#B`iqx zD|>Cfbjf;^2-)oL{UGyI=d%Kn9S5*e*F~`hUFucs+SRUU(9epvvZj^%?2vO!eT~je z>$M4bj{fxD~Dm3`Go;N7{{gI6Btgc5XX{_t2t$t({1RzRB&!NMwm$1>-q=PSVVZk{_VeG9qd zwlE$TRwk$zey>+hurxU)%nNyWHP=>&3i;y9M4bU0{X!_nt%2%+!anXRc^l?Y7#zzK z5;Cr8)?BH&`a4pukSWuYBP-)fb9f3ENnH&2WlD|v^_pm^I!Xel1RH}s)H%km*Co(5@o%E}nv_>zR@!XN&ydRwpDmxgzx#e#AAEFjhy{MB(UB?;9^RtAf zi)QPQi9Bzaw?QOMZ0W8OTaNEALv!;h8#rDvy(Y0_s`O(4qqY`3siX1zXiyB_(C%R*j3uM7f6541cjf zqT4*Jf>w)|8XXy`4dh|1zY7bo!2@snwVCct#Jjs&#AQ3uJrOMt^UTsL-`*nF;!{a2 zn22ZOJ3)BKaluc;?*w{Ar1>~tnflF^XqZC4%o-=zAI4h!3_R*fFn4sf6u*4g^kUJX zi$);JGDAc^sT4EsPLG9U%&k#n_vVE0@DNPr>H=iakl-Z7j;IB9Nz(0t&P7(M<%Av zUCU_BD<70ytk$#k%}!GgO6JfffUMNF5D=Ey=_DtNZFN7pl@=3lo$Pu~|tX|474@Jt9VnEv}ky$l&z}zr6g|$VX zMGf#Y(OI~;9r`zErU!Q$$TpE?vkRISj_ zJ8qSdOLfY26$^`1OLl=F)XSSiXY-#*S;aUYI~eOr>rw5hTM|4abW3&1d9yzY_EiQM zd*ypT11d$s=~mQCs#ty|lq^2_?WVYfw5&5pty~x=L0~!f?*u^O0xf|Oe1cd4mSg6S zwr5EJ89w*^3AmaOoFMm@sprg+f>Vk)0UR$^qcn0X<&)+b{`3RdG8s2@_XIJGRe&vxE!Cxc=W8l1Q5J#h+82J3Ut_s}*G- z`^BGQI#(%@Ej5`3I0h)M6d;Vljb|RS4P~Z=T(rH*Qx7hQXr?u@OxZ;{V7LgieQtJY zU#E&2`#Pm#p;=hPG=><;B&1p|wPKYWIW6c%Maol0VeKfU>8vr+Wv5TbL^cYShMdq@ z!ZyXkEQe^F57=zlUxz}lF`I>M6c*>^<)r0!`CNu($SBYb4y_A5?S&LHZ?uUI@ME_& z-+q>wQ*VOi$k1r3JU?2~!bgT^8ETi8ltDe0(RRztN63DR>QS^+boQCnI? zF0(Bmn9p7EUVbPTdQx|?Ps%_J8Sa@EmoOuPsQa(Ajux6LNwG(1&95r2jM6}ew7uIm zTKGai&uERqXb<(7gUc?=BWgfr(T1AX?1eVlH#St5@S?#q3AdTI)>H_r2S%}k8XjIk z4Ky^Dw#z#`DDJNJtEzLttu_`@EQ{~9umF@M4M;tkychxjTmEWyl!4OPAdC_b4imdr z@83i9N2`RJva+;|?9+`YXqLx`hcRDKMnf3`T-?R9A!(vk4XX)yjd`mV;eEFb`_JJc1eaGSERVj~G;fK&z|=X9gB1 za*j$vJPD{d;+X)o)Fh%`-Nb<$q=PE@2g|5oS%HH?DwTQ+R^qb)=-GH+nxlsqRcjqH zRefI6Rby2ff^k;xwME>Rkz?@jE@d-WH9rn7{-78_L%YuT*yKb4-#=QWror5`v4VqZ zo|VrmvIjAxfRq_e5_+7mUgaq~2rZq)0m%EJUcK zw;DW}ZNL*YCdd=3ns6q*)eg8R-?|u4%-5sEyIdxzz8qf7Ou&Y_x-JNZ(qM}l{i=s9 zj5OjVyw#Rh6IeZ1e^?zkgv9{80!_`{oIP1@doNAQERtxh``f#h98U6DuJ+@ICrcks zw%A@8n}D4b`K}x1ov?fl?X0)2vwpORJUVztwX2nCtpn0j8qEx3FeMn20YNj0-D+5l zKN}DpOW!AFv~N4ZWq(^35s^zQf16yv2|&JGW-wn@d&R4nod4Ty{ijh{8m&Q#x<8xv zImuNSn<~UrnNuy$)HqIr@JlN{Z`KGxUr3)V@+n??!P5bZP>*K*+ymcXldgZixQQTw zjfU_9RVtk}H6MYJEqtaNyahzxULG!@>{!?uLIl&cHxQ`;711Mp_d^gpCc^0=zXzjR zibO{XoLQ1Ths8`CMlcYQ*n>oVtlMTkqgKheb(F+0 zw?3StB1CvA4~!~Pq!pqQQ-lK|LK{{}J1%1NMX^SglO$k^@)=s}h_G4sU;xp?ETE+P za|{P*@q(@-m}!czd(PLZC46-`$|r$WGD%|!v3M{dOC-wK3oi{*cuY$EkM5?;>&$G2 zK(UN;ASMDsWtd&y4Fp6~@45&f8d7FT{KcxD0xj`88)1zV3XPIy}BfqbTq((BWevXqLPn^Cgm12EA8jFho6M|iJinV}L-Y>01xg;+R2Apbz z5_1kslD9jl5n=B_LYhBAOp{@E6u$}-ql<>(>Vc`( zVldFXhfDX);_GAuRopWlb0f^~p&Sn=-8=SpK;CYRPwRr&Fz{kT4-DPW20j&TyD`=e zb^OJ!t=)PAKO|U@j2k?sF^OJHhrU%BjwddZf z_2&rhxci*W-mbgz;yz%)`EdN0YVnTB{RQQ+ThWQcF+?=&RVw)$$}jH2&QHe4?|WtL zdgeQGfB6Rm?){O@x8<8I{U>VY`1`PM$LDju1C36*_${hi@JFQQuOmcj7G1o)sy<^S zy0nTm`rBz~q=>Il&`4$#6g|N(gf@MF6wg1;ZrH1dr=J`j%()7kUZbIj`gqHVp?Rhv)8T zNYP~i+We-rnyp%XRZ~@06p7l*n9gy5?|{K14lK4UA2#1)V-L1p#(adQ8-Wi{%B0`GDk z2Rt{7JiBxv0uSr0D)y>I#~&B6uUjFDYrVIH%{H!A9OT{Gd7)-wzCMw+_~}zWL$3y} z7j+Wau>Fu8yFQVm$Q>Q(Z2eaUcDt*&QGqEr-?xgj%ZB1+%6DrnXEp-)Uh}vBrEY6mCc? zCPq`kqoSN{K+AUacB5gLI+fi!O{hyR2!+0Ry!u+k8`NRfdDGMj%i65tw z81~Jm!5x|3o6pQFp0J9Ybuw^ z3$Ng=h9iA|Y&#%yUZFiuRj^UU1)&Yhii&wknu)m5F98IgVZ zM4amE_~rish}(flW0&t4<(32N!awMu2M}a@c*a5sQvTKiG4zqYeFL~vxb(6RQKnq) zZV``iTmt{_NCV4?;0qvcIa;;$2yPbz3XUJ#a6M8#1_5ebecHts7`)qQe{uVCh;OX2 zk8IJm<~#I(Mn;0QuQJTgwud^>ICXI4(q%KAXBO=impF@4z7{s4ZOe2%v0Dr0`%?5> z7pnGKKtWLU9cOpNO7bYd`*K_TX2l8@F$`r!BmAPAOU1&1;oGo)ES8luvW7%~kcJ)J zo1=(S>ZS6=LHX2yR2JXrpLef^K> z|Nkt||4+a}_}>5zX)^<3dwoYU8*3RGYn%6a{=XoPcRb@iArDqgrgzALk&%dlm5GRp zlkvY}JUCgH|AFy%AMj6%#~-reUoswlZE5w7j0ekq)oWs7;ryQ%kC{>HT*l838(-x* zt80FD%tLBWhs03`4;cVh4|zX)q!cm#fKolM96A0q1KSLv3~^8ATEE|Dx%-R}Bz0>@ zu$-$q<-qS)0}Q`wx;Y%weI#azaq?*Cp?z7`Nu?eXJioNle7<2!>z}0=1O&J>7z6U; zQmuI(7$j~z&YoA05?uRsJt#8Kv`P`{Vi<6{DcCrbp?P3?%KwRffp@ ziK~i@7p4a#&{EV?L^@DL=6K(v@!lwY|Jbo^y#kHj@olx_i>B=UZEmtq5lp_T&e z+%>_XdAX!f|0P({gw_w4K|6u+B!nwCH@Yj@nmNlP{Hs)x7J;>D)nn(B^tt&FwyO-b z3gXnXCcy~64Avt=TL2*Njv1thU#PpmF^Loz{7T3C0>K*qn0z>7us3>yOOJhpJvPkI zbvvx6%>cN&(n8bKiF6W;@3euds#idHsBxRrX*CxZYNc z(fBVShvkp>*B>_d9~r3s4IuO1MasW~WH{bAr+=qp*xy6u|KfoCzZm<+@BcI7|0_lG zR}xeFM~djLobXQ+5zBvt$yxqKnEV|)V`KkEis;nc&0T4>?m}musUj_zg(aCQb_9G+ zI)ImPM}Xj)%Pz7wv=7C%s4!@W0bgNa7>N&68iG%F1^3ZE%6+lYfp=k zF@JN(sVg-W7XZCUB6o7UXavKj(cE)eqf+rS)BHnwURVlY= z$iK-X6CNVU8=0D%TjAcnD8xizHiX|wn(&5z0-Yx_c|z>I8y{D(|b_H$Vhr(mW@$ zvhzc$V`3qsctZf@R4xAS2hW4|75v~mu1TYMwNu2!UD+F|_F6|zBEy-3ESw?u$ibnp zL3}rbwZ>Y(OB{Q$9tDP9XdH{2$ z1_gaQS~r?fJg$`;703Qv+~cclZ0rua>m{MSh`T^ie@y@2@eah*0l(Mw0E1po>!(Ai zeJDneVVVIZwGa@mm$Aiw%01+qP|yiDWeI*{BR-MVhk4dGAe#89)ed+Qf=M)*0bynY z`nfGbl-qsy3W`0JS$o`ah%wU@yDLOURtPBiWoI8=fbh2ACzS%M`U5Io86u=){JLy`%j(eGKi zTcF$GGws%F2_{-@5RnJV#5*l)Wll5T9g6|7!GiwD#QkKqn>O8n%Uh3ccTMDZs0IS zDoWZwF|G~_8Q)o2n$71%hOjkyL1y0zrRg(S?F1L^bGm$Hzaf3KJ#59PLC!6d-6e(|b5x~=}Rbb%gC3hvn=T+_5S zdE-j)pc|J#)wH!;p7LnfWb*@pat&CvJelW8+HX?cQ03@kc;gvX7IPD-Sy@T$#hf#I zXWMHTU)kzbYB_U+26o*^%HUL^45cei_(Jy3)zCrf#sa-uA1T;*U!f* z5QBg{L}wTUreSOq4o?AF&P{#*z*@qgwI^=(*=NEiE*0A!0)hAuMv1h7!=@auWcIyM zlmn>`G9gge)a6=7Vrk{O{|UuUgdhXP+n$7PwT}hjGmA4HV@WTwRT+|SWzvCp#6!l) zCHSLiK0x%%iM($E@qR^5Vzc0jJ|hB%fr4=enviVOcr26N4+x?<e?)gY9K% z_w;i5T7u@_$;@L$Q@c1D95C4}BVg4XG`f`sF6l-my?3d!Ri}bscyc2cK4o8+G)U1g zgfXh27o|)u6^Zs*8ll$XEPmJxU|&2j!tPJ(JC6K#LbSZAGTa>$uvG-B%GT>S@03Rj zhK{WrYYHvsA*7V*4^zg)XQ=EJ3R@-OfF`T9|AjqyvddO(2CZCrjGnxt*g^l31*#3k z%}7{GL4r%k4}Ek^RrA@s0(D53&#Eqf?(IA>Cj*-z9G5)5xR`-2u%#oy)WhXVD-%lu z2<#9VcB^$I2;TBap&RgGT?kevG)@EARg_@ToFvjJN4R_R@Z;&8uHAW@#G())lt#+w zffOsqj1yTOMCsHq>b&7DP{17xyfwmuZFiNPBT{O<+;c{6md3$a?<&=O1S9(xZY11a ztZY@YWy|V`s2CkGJ(2>>jy6h?k<8{TPiEwP)o-U(*2mL_fiX0bB8*;4U0RvtK31wP z5|?PEbTAuWtI3IaCZDUJ{Rtn}7-`!+4LfZAvZP)rp$O)ovGwY}X!RZv1c*vUVyHe% zF%MuH%jV`<&P9uImZF5=(0!!SOYWt@)iXY+Nt918V^^s8p(3+&qNT?$k>jCu1%4Kz zLKG>k7uca=S?7h0)YYL=g26}xi7d!M)v7;fJ0D7K<04G55awYh77y~>ikb6{4PTaw zUy_8_T@vr#D}dIv6z(kQ)D04J;gXGX$c9-ldJrloLRrr0bK4m*- zJYl#bSDi}lUP(p8QX=RW?VaG30mNeb1bT9G^q~3v3mpFKP(eM>6=F<3{?!G-`MU$it(A{7tV1in|_hZ)E$Mz36TNhl?)Mn^LOiIM?_CJ9#!{q5>} zSJ(Y8JNM`$2`yPg#gliSlOR9OC1*gOU=WYy!`T+gMQ9`XpasR%80$g?WZ^VX3Z!T= zC4y{zC*$o_NJvlCFd7RSy{W{dAM?rBHk@&yKR$ASmKP6Fy)M)t#!DEP>@Sy9A>Xc@ zY|m*=;J&E!9EWczPyP6-?0ALF=B>PXM)5ice2}M@dOgb4NGBTzKGrN)j7@@>jX{^G zNpQPOePt(X&={nxvNu$qh;YBbFm$VgxBg+siJh9bq&9?!i_4^Do;Y&bb#XW`KE+-k zL56JWz|r;ral=?_qEj6L4>0r`w4AIX4r@VbC@Ab&4{ttf+rv3$z&*ptt`~REmoK$S zwd}gC(c(BJMXB^n-OLG#oZEss?lbM${R)qZk*MrKoVk;&js`3K&_`zgUrEApQ zFz;qJCa!2lo4V1|aSI>3x)GNsk(ZkKT^h|wDVa(>nt29tk)Pw0m8eHPm*uy->o(&T ze$mTwd$t{VV=$uIu_uXaqq4|f=Hbf}sx{aehlYtwU zv1Nhb94Ym3w^8@irk$CU@CLGIo&lX+>`{;`lGP%1n%Sy8t7Csd5&G(38?2h6gnjFp zd$+0%MdB9q`gy=uu%%LEgT>##8x5kO1BPrO7D-7m0jIcv?BQpGm;7zk2cvLNW=d(_ zVEyl$u?}3TkoEVWE-v{+ef=WCu$FZa68)nMX2koQ`-zz9@Xfqj5%19QZ zALp-{%kLuZN~alKE@iT@F>T&?!{S7uPaa0JzsCbBWVKzdInF(wx!zjdToi%d*Dv#` zdfubL>L(z=rzrc;lx{i z6Em>2w0zqU@*&tblj(MZ?*0ZjOicaCjD5z{M||Uqd_iPQoV*_5&g=5gA|T@PTvmiM zFlI|ZA|}qk-Utzaq1R$xyp_w%TDxHIhmF^sugVb!b4|LxLDX2G<|Y&b|XkczTGhd*%BFlq7bA^7^5Y&!`)DsiKSjB5WjAb zMrA_DG>Eq#RiQjD3cr9wUElGYEeQ{&xA8*(mqj&rauFx@5-d@qD}-%00?ytLzkvVl z$YtCjcv&qm+-a`saLBy@9@}(sGI~S1KH9yEkDtzYf)1w1vMD+7=d%S1EYvYrxq{O6 zYFds-Fj~IH1v`u3Sc?1RnpILF;7n&RB7~J8I&*26lfJL(szBkDf3acT-mhXEKinSC;3%j z`k(Nvar^@8B!2t>yFq{f4au2`41J&0Y6a*qBrx0?G0`b%fzH7cm=wv<2KUGOxQi8{ zCy||92pirOmTbPek8QSra^S=kU1qj2ZV$`3vLr9=u?LpL`~Ev~vK4v85Xk@9p9XYQ zg)h}^YTTCN*L6QyaL5dQY^g16tGkII$JJ5ez%5vN8kk$Ip`R=;&!2AfzviHCdD`T$ zzEvHgO4i_@BJhk0d{brm=U{Ou}!wlB$`QZT8xq`>5ADmCrkxa5KDRkn$(h#sPZ7z6pY2f)8$2OaS?SwX{iyKW| zNF9VTmWOO3m!!eEpTZn`8iH8+VJP4fvsX2y+Y+E>F(73eo=KSpEoC4Fd_0r#c#9sl zn|XeFNm-lKKL4ppS~30={9wf!zd8lyt}~&={1`#YWqcYcW2%8abEf<4{J!wp(oHYk z&Ed&GH)rswQT?S83k^qLa8!pamYzEb`NwK%-AR%>213i0yMoj+f6!C?YU{g{*T-kn zsSc>|Hc!RO;x`HYd{uX!d#)aSTJLq)5sEcY>3W;ZNGL{%Mr~Po%Djoh zvGNr-uXkV23Cu6f$ODBuE`ms+kfLyuXiR1tQXe-{==wlCBJi)oKVSv4p=*&pSLw2W z+x7pv2?~#wl_y9iPT^IxYZ+d|V8hTNR=)Tpy*K&IAL+DU3#KT)w$a7MB_BW{8bH%X z_yKB;aOGly^bu*kVmh~Sqj?q%6#Hnh;BtMpnc!;^giE;Sa086II`MA6IR<6Y&6P16 z(W;Uea!uQ=IGJCgq>fr+fGfFIfBHgAp+j~?fqi5&VT2WxEf(O*d=i%}4yhcWNfj{l zn~KSD0>uCWtN>LoN(SHL^-jpSNuhDiVbz)uX8O$HagWPxdyx|0PN#cy11TB`aXL;r zY17atNGBQ@B(1W7&dDdzEaUSLCPAtVRh2;?A3Pt`NNOYDThvAvIHANv0JyEZkMgs!7fFK(hmB>D2f&t^xIKz>sBg|%(@Uf6`5t8tn*8e&Xy7kFKR1wdR${S zs%4^+Au8fX_8LuNF5bM{-UCQ(fSQT)Rwft$Hf0Px|4?fd&N~La%~qiEneN9 zXJEar3`#-}?IhUzUFadjxM9B$*q;5f=XH37;bOLw3fVW zwm+RMDl9WQS&qgv+*<$_dvu>hjx=qY_{`}l)Isq1-9-!--0}o%7k(I0RrwnD^bO12 zJSkfai#tkz@j*XPP#e3U_(1}hNE+?~ID|@%Sqo5x?p{y+gHWEAoiK!WH=PU~mFrL~ z?p_G(`@R4Z69&DDe7=F(kHvzWfr3ZANbVN_NQhOC{5PFed<#3ib$mbW1lo{Hst}pc z@ZAr6IXo^%90rl^6Hq#q{E4C?Jotj#Y*Mk%Ar))IlpsTtq=jh^YhIt!by9sGDAgBZ z`COtV{n)-Qd<#ZuoVHBrbI*cO=%3(HPv@dem%@#$ZhF*eVk2`_a~wxMzs~2qWa_|) zm8`1-pO;pQ#&H6e$YR%6P%>Xue7l5l-1ua5J@lUS%LXQo^-n%EO;4Gg_*u5SR`=iZ z-X_v5BBTpu)0h7OUa|zGul{nRoR<4;kQqvGiD-|ao`o)Hq$FBkz@8ijPZBm9%lrUx ztYcy}S8DE9#vzrhF{fv~7BLCuqA41oR=08?e;Xi#DKb<83DeQid8L76mpa1Jc$%yi z>x}>S<|)j2Chz4aVjs=GR*6j?}?i@6zTt~jL> z7v)MhkwOq~` zcZEk0^p}?z?p8C$)kR+7Am-aATy8f|UKWOC+m7cz1p9)I*e|DZ#+w#eQs<~^Q;A}c zN9TvSh-Yq2Z$eNAK$LVCV!!JsfdVrD{^*mvnJ^_D6wyd#v_dkj^f|sA{>wX&iP@m@ zx|#4SN&UQJdBp%lZ$!C6XyCv$@(A(yNpRqKv(@T$A_q4JX}bM>c911e$KgrF1AAq3 zyyESkx#T>VXJO@p7j!gPsNtjotS@x#>koKw{dyE?)#YAGgoQL6cWp}Kn^5hpN7}$B*E;v&k?TnR)erNj0%?B_dI`-WDgWgo zr#H*B3S6($D48uTk600UpPE2r7dCC0ky{^t_N2NCgBGNgJqvrIk`&{0$T{qVShH%< zg3exbqz)hIy@ge>I=*zLsV|XZ7lK}SYHF#=ZRFpg{>~s~x&+j@#>5-coU3Io!#b!B z^9Eo>^9>XyRN7mj)qpv#z*ea18q9 zBwOnYI599BogeLX8FsaqYWlg!Wf)zKDQeZIh5~ah$0~kEmj<1%8yK)I4aOt%0jhzP zsDxN3XrC0RPc~?J)={Fv=6!@W4&jrbJ*QEVy`*6!Ak4^c2N3N;ChjTCOHnonY2*AA z+iU}0k!?-=@izvi<-ns6X~V55uenIiGGl;v)iNk8W zpV|;n!MGxEU}U%RG)b6s0JzJCQCtADhl|$_ZrpVgLvYVKZLo;J#os zvTV9>rZ{ZBW_+OeH3riNB~*j9l$87%u7>nZvi2Mr%|&T*(Fg4IW<*b9djl|5>v_ck zuh2PVigZ9NbU|G{@bq?Nw3qC+n4L= z1#E*X8^KyXx8NHalZMm=^T{_I#=Q=csSgU+di)6t7nP@Xy5qvbfsh>nT8&0+Lu3DYr?l6oN>XU^j91rZc)tt=*hGr?oF1qu!8NJb&dl3H8P>V1 z>=OKgr`fFX`e+G_33DOO(0xwF;{4B!c~0{4P^+IS?OS110yqS3W*XAo2?cb&d&!rw zY@h(O5hQFIIfzJT<$km|z$5W=-v*Db9~ghx8?=d8mD<)QiVpFxg&WpZx4zHM5TG0B zM)MVTl~uc)LG$a>38uk42XYm=4rQ@y;gd>oKO4=}7{N{E0(sF15e)zl-Co8>5pHn$ z_oE?$M?0Cwhd_xp;hlt|b>0`6JN37tyGZA=yem~fco(`4_kuv%%0Bcg#67K6G1u90N>6lYdl zOl*8rYIH|^^gdEB^6nIJ*|u!W7}N0|)$h9=_#7!5ppIl`7jHTolq39B%y7}zxnc3a z?kRcYz54d+z~)`p2ziYYg!{%4+R~qI!*G8x+TFM4m=1uO4V1vAxvYU-ghA+2IY;u} zKDzdCP)D5aF-{xHua=gOlg=aF{$0~!^`j;~`&6q`EuFc4`l5ngTT;Yuks8L&!5(ju z)!K_^0DrgVGfgPP6c$rH#={|#D)Z;oN6o5@FKo)fbtTMonwDxKGxGM=AH@$psQu=5 z6Z8~chdq^lLzga|0K$B|dr8!FJC=z`(RGDk@7k>df7JbDeH%aT@$;yHiN=|{ksTm> zibdjJLT4hzkx`Q+Gy+F4NuoB=I&GZL*cY@)A{i5`lWX@j0r5ir<7cNJ=7^&}5nQ!a zR->0gr>?f`6ogIl1_UPWo=;}eh6<*oeJ2iPZA;b~gfcuJ46cnu2Lk;{^Wt(yGCSryuSJ|1>^ZC6mrXvyVn+P0ffbs!- z;bSf%y-u*2R#PbCs}S$&#I{n+C&pyb(q`*}?PHKL7h?Iwzyw$<-8NsNhZKS1f%6p4 z_pXasj{;yl}j~4QPQHs|GTtJ(s z7%Jbx=s76(c>9t+sM<~cR0EwSQM9JVgTxNSl&8hXP-~Q!Bu#dux3f5-2R|L{?I2=| z@ol`N#DR22{#E|#RA|_t{x9%5hs5AGs3eR|ktl>N!A|K$Ka7u8q=!alY8KaU>sp6~_&G@$gL1mB5~JJF!N|okwk8 zSvPjWnsE5Bw2}Q{_R(gDi=E?2ZNZ-xLqg3#p;^Ynh7z7yH)o-5~Lk5Lv;`j zsWtDi{nSBXZ;l$U$<9O5>OfQLZ83|L`jFjOah+lliF|Xv3OJH=A>fBp0Nx+&MTz_b zw?x$J0)2Dsv4q-dBhw$QvO++7Yb6!r=GGGG48fY_AkE;^z@a^HV1_0qa= zw_%Y0oLxT$jsk~02i^+a!uWfA>uR2F)EZ&n?hH32fTMC{@;`l4wn4r}71v1jxvyw| z==;!Tnb*@--`8otG<*cj!y2!mgqrD79ww-p-k4?G# zdtBDFVL$_&*WDWj(cL|sa*hfNCnHP~Os(o)H`4Br!27j(4fl^wFWGvzOtQac`1VFk zyrEU#qTCWDT7>E*5SG8ixlF{3;2`(7B|tB$<<&$$@EGqQQV3_!exCif_eF(lDNT=f+Ip_zf(T{U4$|hn!N2rV5Az|bujm&ZH6&b7kWDSu01nny zRYon@xv2wySdLBiVS*TEG3J%%c*RqF3|oQ%@oePf_s~t+Tu=@SAA^^qHtreg{Y_** z8f$YZZz|e*@h#8~RV6oEifvipI{(S)_k*QR7OSY=OgaEdPc=8?IqM-iNIO~aWzaYF zUHRw2Hds4TNBmpUhV3*1wNof?y^Q}RqFonAP~O*6>VNZCvF7^P>%8W6+uP8T2q-yU zhMO1q`o+)U=A*~C#Kz5nnCApA%abxrYuhx|qBY`;I6`3Za$Jmk6?}eQA?Fl;yRmb^m;&7?120yW5K!V_4uR| zZ?d42LzUc5Bq}VKa~$>`NYCnR5AqY6!}ZA7 zi15Gpma{s30SDd-jyGP7uIj!Cyiv8ZTTj0K{~>aHjInP!e&~F>+|&^8t>rsV2>9~g zoWR!p^6YRtM3PG$0~db*1o#9rL+I#v$;!w)W^5L-{fe-OK)cyeTbu}H9v!G^w(T7uV_$096#UgRycBT>#s_u z+fUcX{r;9I_Kn=uwDPN)7FQ2wA-R@4oJi#TT#>?JJ@|zPeQOK+r(4&3!R`4`<1XF( z=U?C0tu3_utUOZQH+41lYA~_$?zr&SN4ewfMT+4y59kPbrFdU+$w}lM4|eF&5p**N zz5iHV7p`FuGQwlY8*Yz!eLK)VnyykUutw@otwroS+`1K~tR8#3L*V*tjXL%C0zF=C z04#daDm_pCLH2O?e{{7VrB; z5LhJ3MJ83Hf0s_7t<>ivA^k$C6)K2tDjO2JNmt2T=h+@M3XK~13Y(JDIbK~df;5q| zUN2{=8`hE=?EV(UAmz#`JzYxlG9N+Sk`AjOdVitfW-}xQbVLg|>JEIR2$~gkOH-*2DCW+_G04Z@R-Dx~3@%SHQzqAl!{?Z6|z<=j}ravd}{RC}_F=2(& zP^`6&32qcZA2Uri^X>R}=OSL#Z+9zjZ`G>(SXaa7tNq>@Cav#r8@+h)g4_hZ&Nq@s zB2C_27Wc^@Zf26wJ28SBu_kHC%LUKw@u;`kNC>YU@u{@G5d7Gt2XHtYIv%;O5FR+S zlZSNo?Svud9O66It@qCbsQgLH{b_jjJyLo&w;zPTfGEU1p&{B&^{itKJG4hHoRKK& z9*!pkR}1rT>_#r^>Z0WZp4UjT_ca)Zonl&0&X{+|cL?jl>m!6a>nJONv9{s1@}GM` zdwmNer=6z17jTRsycc{ACy+?R8n-A{(^pd04_i9tKP%Zwo=)+#^y-+u%@A|K;$iaq z!b`;`+$^vSe@=QXcJDKDP~onyJ#%W(ZlWLkS@@J#qI2D zS+UpF-Lm{ta93;aeU3(s3J5ZOEO?~9KD}<+dZHSQPDn4vr;}>oJ{_&)&tINmkkLH@ zP64Uu{qgfY-_86Y$*SeH(`^Bhd6IHTDXh!Qrll@PB;Umd3gaRjB{tIJ9nCdf!j^;k z;pxGjfngVUZ^vszQe>pcYeg|1_35$};^yk!Wl0iG9$juZ3k{=hnwreljpiZqD{Q0l zmKhKAw#;R%@BIU{I1DsZ=ngSJffvE3_QNIX(v%=$#-81)!Iww6j} zGgus-=0fyBY{P}TN2Ui~8R29>jhvOrlUu6hvI^UsPQFt^rc}t%a(zxs1+WYgj;Cct z1Og6jrg`tRz8p~3RH9ra$6jh>bN;SV;ZJAP_A&~+vbydKuG>`5P*YJ?<9|D+8im&n z9q{GEMsvA9XKH1AeQthLd2uDsgV>nm7mw=C$nWz%9qS!`AE!Lc%+mMHg_7LIsjcM8%<5j+v%01)>a2& zQ~s@kA7@t90w%=$+?tPAEKyMAat^A1*_5@j40ba6%M;gbFY$!Vf%da_i{(cwM^#Nt z3&Qju6}&A;OF#UIle6{^#Qe4v%t4FO^8rEu8#iZFe)dz9R8390k%8Z532Y??pNRvX z(0bS!Ku(6*<3oFZp6cR?tXv?_)LwtH`R6fg%683^PpqO>65N}*_S<7d{k;-M@k@!y zSa|(?7uWU762Xof8){+S$G#$%PCQ*kzBP#Is+vMzMOKA_v4Hb=NY~Pl zs}TlG9{N3K+W~>5SS4p`I_$wjx-QCkN@y-t2XfXspJuRks_LFz6zV50A5C(&{Hhud zb-AQ5*qPKF>B-u}r9}dh`rEea9+I*Rlc6LTpQUP;lgD zG@oMdL;Qk0WPv7fG2HZL74nDs=j-~n*7q{E_OMP6IdMpI@#XzzZDgSw4?y*c|I?w7 zhdkh}21oLa#{Dj)q_Q-%_U(K5Z|M2Hy-FmDCAIQ+{rpBu;vPXr<$evtc2Lb)GnUc| z#IMp`g5@d?xYTiQBe-sv-L8(6@WTkszzQ;ph0TUA$`aA}3*jG0>>AODk*ODbEX&y88MHRdpOt&UPGlyd9rh-JWyIRwi>x8 zmmfm0MqXBXh3iAi%^M(H!#^c^7;w9$zV8Zb zh@yn-WnT?l(7zs^HOWai(&3D}{#_hKy6;NjzVBBx9B_Q2TYxD!;P!^Z+)#+PJ(N*y zeOvEKu>NqB8USU_p^6u3o$Jz_eXr&s9eq5IIJVQY@7h%@(nb-=q_zrf@s0qwmYb6s zd>ql+0lInI#|ZGu+?+CrFXJHT2UBT+;{gnZ*^<#iu_q7z%+9Z z(OfPfU7_r2a`N}D(8P34Rb~Rr(5t?TE*%(TP+oZQNI#%b5J7mzhZaxXxlpo%$WQ{a zCv4D67|h9f~lVo#k+~4#Ld_Y6W1SGj#cvo+H`nlNIPyFH4<2$v;YyY=bh=IHA z?;F1F>Bo0twq28h&1HR+tJCeW2`baOWpnIkWg~*{{DH|??5DbXJDm%z^YQJ;1|9zp z;?T0sa&v>#LcLY0%eBm6=TaHbI{1qT_cx@AueJqHfYoaSr1{BD(F9mw>2u}B0!~iOv*0bop}qid%{ZNy`T?yj2IkFH zR>S3jzAZh{_C#$RwALJM%Eq0m65mACNTdRc7R+GI7C|Q9?^LMdiVcegYrrQ4c6x9t z{j)}MtIY%HXGxiocJoeW3`_Nn@rqICxy@J|p8AB)8K*gW-;KGek{Ta};%Q&D+3XEV zJdo<_0QkUfm@kSUZJ}C34E7kqp`71>$*VSq8gC&=@9oVW?h%L3?Q>fNFpQey%i~*i zt*-32C{k%FPvSmCxtWH^Ke zbA}UHPaqn(u6)ZhC3b%|O>X%kOcp3aH%+QY|6pbEg4zGw23r&cy1Bcl7!38ys>p1I z_PEi{sfbkhNGIu2xz*nO`U)^$JsdtJbp_$legH}keiFulmA32k^ah|U;U7a{8R_yv z7=+JH*d*lUe@1!IL-H6r!#3HDzXg6MmPy9^S5()3=v}cev;GB4{z1H8S^obDUJ3nw zdC2ej)(&)nHkL+zLy`ZaeD!ZoBnLYY6AL5DyZjXk%YTPOaxuQslz#}!f0w>`mr?pp z>8n2j|1}r+7vJ>H#1s2}a%-Z3xM|A~u?8;wZ)hU|CF3(>z*$$b53A;x&H zkLV$auy^p2i+-y>2vgTdrww7Td5k#MLa|x18=rZAj&k9)*}?wo{NY+gcbd*qc52u5 zD_P zPyM6722gk8QETb=`Sv~SN*80Hdi8R?b|bw9pLXAPqNZBa(e4agF-kMQDqr&%PVdfAjstT&ReM9WiBbkkU*nl-qWa zBU69}V%lQBuT@_877EnEd!DmYqXi6WTw?06lm8rZ+39bmxZ1`a)v42CiT3F0^3XiH-7jgyo~1&Kj(gAT=GJ~wK3_J!p}X+DaPTq^ zzVtV_UGmsI3KA$Dzs_^Q0TWE<9uU5Qn)%t>uc2Jw2wU&z?mc!W3(t_gg2ER%l4IA6 zg7&x8yJL9)z_8&rSl*_4skso~AQ#@&=Z>TfyeAgG)U8KK!eqC8f3mOHG}7idFWxz6 z@%CL+P>qZOr{kypr=hmi`^DsJeZ{vR){BicT!}or$f7<>s z%KmSL|GNI;oRj509RF$K_=k!7dH?TY{+hs_NpQT2;Qrm`Zy)UMHvjPh{XOx&$G`iA z<#?a#-yh=d^I!h{n&-RCf4BShm_PRZp&#tu?SIaH?)`1=U+|d!Mb(YvPcHiB?DSt% z-B|udRX1jK4z7RZF{60{}V>-}8R*93GM9wfod_G|X_Uvm74Ex{JC$yOe%<7SrYA z`+dODUowMJ)|q6$tZC&`I2@aja2Cs4^UT%c3yFk{Gk}|*E6>?V#hK%>gPxx&8i}om z(QLkQAH1t7*KBeXXHk)=!DaOrmRz&14vqE1^j94Awaq9+?8k6HbId9`R3y=z6d*M_=?L|YVm9%0+{hDI^V8*j336Qbw)lPB= zw)piX5tf)CI&?0|yDUN41fx3TNS%9U!7O+mhPCTF$g~z%%P*3luRYiER7`u1+t`ga zfBk`PMJ!>X1%vM!H^;T=3pmFcKTyd+o?*eC<2U?GJ86aJ5`Pc+FT^H08poT~rp;n? z7E|d=6|S3Ri$dvk6UE-yT{OxtLTBaQmM8IsqLDOh21XK>7w^2KzN;_PyMEhK4YP-J zORuFQ)6q|ja1C-;UMMoT5W&$Ns!US#A&VEZmpb(BtGwX%eXP2#C?1C`aumxx_c9RA z>nd~>Ki4S~iH7WLsah%ilwtLY3WfR>@OeNU40U7*i#Y&sGcm_T_ChM1(OCmMfkW;h zF#jO2)SZqNuIZ#R2q`&v!B;_~D4s%eSe3n2u2cYLGfRhKjlW%o<4Yvo%wg55R<3S52m5;$SzQ^^y5_mX~pvL z@Qb3Wn~a;4sm-z=(CD^-kQHkwm6D`I!Wcd0;Kai^mO}#|$7x<}=B2Q?Ff&{z7qL_@ zS`uQqi}${1x8=eNHVND%@e+HPq-(-};5jiW z4YXgb7J}7bNCT(IWJ0}*RE}FX;k?sTSn=)eD3+ozeiW|uBKD#n9v z0dUnkS`w}cUPZ8&VwO!(feAC&uPufrYh{A|)Z{kms_g0}9}{YzPAy-NKPTIT;*oN$ z=e5GpmJ|#ITN_0lT;D{QmGvWB_R>O#46mA+*H?23+jljE%HA-H`_x>gx5l0$#cB%n zvEK&T2CF%@mkNqCRCBZ%jM1mGW(IDf;Qrtt!~A7O>Zm8De4K(;eE4Lf;x1qiP7z|% z!|4k;$_<=HC716kmF5>^SC-st#&X*QU1h=ogFrl-sZ--F6q9P^8uKgow;eNwlJX#w z99DTp!Lgo?Kc1C!AP`{nlwxzEi#ZtxMZdAAi0`UhMB@5pD*TLqTsJY|EXnubu?|y$ z&_N9pWzG@G3%)@!umv>{{(&yiWv^svzPAq=Dd94Hrgapu(9IQ~eVI>-l=>0Fw-Z{Fm=I#6JbnK+#q@xZy zwr$(CZQJVDwr$(CZJQ^_r0@5+pZU!@&#aj>f9!S6-gOnOT4z_)u5<0rm*Y>x!nl{T z=r|u!jD$rLw~Ud{81SaSp-`34ROnC`&_vwsgk0Mox2q7l;N>5DLXnqz(wtvETYGm6 zTYaodxKFi4BGgkYk01SW5H`kP^GJVu=C?Y1MwHURU_OjoY^tBADA8dMPyJ-3fLB~= z_V-$5xzK@NnoL%ID97+@iKH)sv+a(o!a(Cnjp%0MINgtDtD}>l!Xg!5u|SMZl=-RF%@!%QGItLZ#mL)muK%7dSGC~Cp^-~Fjqks^uTy0=rNUM!=gV_-2W3F>Z7Jv;-f|+WE5fH zV&FFd|)SU$I?@;P$&xLWk;g;Q&-YL1H!$OZ#-9=}5gQ9i=tY0~&5h zZ`be27q12tj~OCBBpdk8R|$dVpSC9!6V!cPV16Hm(v{tK0cn%{Ow{9xX z_#ggDoyRXS8h1CM)%WeEAByZKK*H1E^6dqX7Zgm?ure^VRvsGO@i3F>mXkP6yG?mOtR(P~6S;() zNDP9JEyMm@=#k0HDi@f$b8>0A<4_cE93%l8+pPzXMzJ{i-EiS}*Oz<{K9EFD z$?7H~mW~uBCj6%?e65ij+)74IZVX%kH@R~(Hz*iNWFR?jMlb8Rl*_%cJJ;)D)rqmb#YZR@ZwdTmp@o=Gc7;>af>GcDgpJ zRbq-Iqg&j6^4h%v;R^s*zQo$$|FA^Pjd|RU@Ipn5* zzY2bkc80TVTe2RrlVVpc;mE&`JB=dXz%949#A7%uA zIH)$#scq9>7Rqlf=9gKtQHLlrm=@M-GK;xkvV9|~ZfEUaiS_O)*ol~YZ8T@14q^d0 zxN_z4E;U>f44WTZS8e+G^flnK*;Dg|vZ@1>Vk|S;*1NBx2bWZGNKmF5NnUI zl7}O_wt{~}3DFY+|1lZ}M4Sr3&@a^{Oa+Z#f*QY0S+9Q7pX&bmA8}H@P?lOj7V_S< zUq8`u_yZh&)rZmWfs{9e`_PQVrSXees_#uZzRRvJcYUJZhFu3iti@&n|8j|YT_1eK zGpqcPg8`}g`-1GCkO@+bY5?Y9gd|tKD#(N1ODLLEmYE@1L%$Yu>>8E)SbT2p$4;OE zmDVYOkfuk%h+d!#!eqFytgu1Qd2zL3fV)j*C~`DiZEAX-nH-IFjMe-C=?q-)^LD(4 z3X2K2b}N9j&$rdz8~b2|A2pcxN8U!ET$!a1*Mhl|Vvy(K!XxD)y;F}Ba~hFiU8$s_ z+Cs&T(9!Zz-9MTtIg7Vs2~tDKL5@;cNs!Wftnr{IKQu(Yh_nR;`Q)HPX?4jJq*GLk zNcPoVOH`xClj=P8&#bA8tB;9b)2YIG1}oFO+K)YlKDoZhqi%4fY9H%Q{@?c2F1~~g zSy(4cyD(O832PK&4Z#Thyp}s1C1fZ&ocVMX_t~H~F7?E5b#xYV<}nHtclBtk?bU10 z>K)UEfkxExE;+N%fm+R`*_HZCkVi=55Ayb&^1E~i7^0{{cuWM0%eb43dUZ!LYA>O8 zXw)CDTyB%~50wq<3fEOfTG}$S=o41a$K5-3Cb8{%(Jg@` z|8|s2l{*A}%BsXj>5SNF_jG;ws7>mMB|coR42DxOY@r0D?L4TC-M7*22oOHh-wMA0 zIKLTxzxMd%h2n+RM=D1k-HqQ! zfkDuH5PYD4o@t^m-!{>G5X&LKsXB&NtBk@J@f09T}fyo4I^&QKpb-C$}z{`m5Ia(+m8!*E5JlA3@tRnboFfg>Ahcs7U@x zLks?1f#p?@t z;LYsK5o#;e>#@uU31@Pzk(mH(J^U?FuaWDWf| z?>N_PJZHYvoYyJfn@`yc8JlEZ>VBV=cxM6)&`oQz9I*s6?oH9RfeO!2YJ{!TK z+_U?D-FHDM?z{EC{($i2fD^(es|Ls9DE-V!n{h~=3Gi&Jp-ttK+4CJGOobpgw)pAN z1l~w2OHU8?yFXa?n-zH)?k?FKk1x0f4vp)pg!IF=7V*UKfM><2EN&Sn?|S~)?ni%O zP(h&BQzrQ(-4Dqy{A!$@CP)7AL!dBx zk&*pM5GK)jO_g-o*qGiyYf^S6`+vz-*{=Dp*Jp|9#7$r`&;d`QBEQ8+YT5{l4C7sj z(c|7-6r`dhxdk;$`f05OTLsGdOt=nwqTis%dU z884+8`?GPLon%j_rztIyS~!j_&NpUu5)=YTF$>LQ zR%|%6v#Av-M+J+91kQy8qUOrhBG}{*Bs}Yi>$fL%0y)?o2Y8jIQSGm{DqrsXpQN0a zJ;ptkNgGddgD`HF=BI=^OJ;^XnQtY*mx4Qib7xOoqU29pI(WxY6t-RZzn#Iz==-FA zCF^_}`R3%H2Rm));|y!F{;dtRx`I36d$oY6(rPRgm%#(nrfd1;T9+46S=RXJ0~mTJ zto6M$>01_Jtj`PnH)T_w@4(5hJTSBivhPU5WzwK97HT#a6Fj4ShCN@%S;}NwKc=EG z7@EaWZ|TmKGx9x@58f>t{u>BbFT(+`%YuLn*0gGNtJ^lhHu?FLHyvSFTkkf0E1oue zL$90Y=(G?H5*zLvEb<@afqKZW$IL*ERurwmW$x z!7QQkQMC`&h6Tc$?#$nWwdvSr`}7InWzfXVzzml4O^S!O`v*&$Zu2M$)oxH_vwBnl zs1;4!XaGuQ2{qu%So&>k7U-?wwX3P~9kx0)J?tfHE%ii-Ld{2`Fc-y6<3QhSRAjB zvVvPYFIHR+%!gSE))OHNc36yuk+HS4u#r?Lt*i7X)s4WF`RTVoQ@P>pv^2&d#*NfF zOkRF^HD5m{*vpAH-!zlr(O#s7>{y+czDhZEz<~aagpG+{i}fX}wp7{@M~zIwVA6qB z{LS{U>T9n0bHry7@Eh)a_bntMTR)=!Y+w$&kC}j08NMH>La-oCd6TJbEvk??zB)8o z!*vj9=29<#+H!v2g4%Xz1C1hdF`=ZIik%R2Q9Z&!FDfM|tctr9wAeck<@qchy*ZR= zPf?(KIV5?SiXlcBq1xBayr`jC5!$l6LJk_Ve21a0HUYL`p5T`#v=QaKS&lmNP=?YLX1r0>k7)q_@X5fow8zD^@lAWLv5WuLu$=piAQ(%6p|H?lyr$ zH@;25mi;E$f`O`z?*%AfD%ordzyEL^Q?aynN6K<>yS#_Iw~(T`q_)`xiaV@LIeL{NCFS{D-zKuhq9>%b15x zm%V##ruxl>cm0>Qg|n+p&j+{9$q8p}7=D~(DCaIf(UZ36XNKmrQ~B$G`4F!5!p@KM zDZ4@YruBBShp^CjyR%?}g$4fO;TVeUy;X$=FLVJk(9kg#S>HF?5v#k(kxTMn;i z>+0D6{`Q{~wNM}4pLbbkoglYY)PF3z&Y7P$tKFBFE(KkaJmBlQe>EdoMPDU3ciU8t zYL8hDUwy4=Fi+FHd!cQT-IsHJttcbpa`TmnBB|MVb?TiB-z|HAKhac%WF7XJbVhUh zV9E&|=N*n$=SAuW>oF!lGXcx;#?X46`poyN>U(ju@$w(Zl_^m>khC|zfEXF2EhYs9UTOHaq1>@c|*gFDz9cWk& zRPoW{K)gP}I0;x6dQVS30=50h>G7!PhL@w`h2$hKg(tk*x<+cB8jz->ywAUaU2w>hZ}nhn9PMH$4y8xg@PeVmq>H?dt$Wb!{Wd%pNB=nW*f*K&Hl<$%Sb1`^LXD> zPU3lBjGtK62Hf|#2jhmd1Cj{VnAdNHsjr&tvD#gQ5_RzhVb#{Go$Lv;*4Y8Mz~T4l70r>lD9kGzc!X=aQzx!Kg=o)HtR>RE$o< z-dPHKY^CL=Sm&kI0#_u0iB@_>9A}{`5qD!j2fgu}-bR%pwbOmN!ABAtI9t3!)b6Gv z!7qek*0AN3H@V>AP~3uT^$`+ke(Y%8pjZ);h?eNa2*aJwc7_{RMnz%qsjUgifWWe6@x%UCQ+lvs|5 zi>?#}5Z?>-PlbradKM>}fZp!|Vy_xAC?2*mRo0$&g3V8joPs||x}aUL&lXD_?@lwh zH|&<89v_35D&0zTsg`>t62>)~Pn*nfU34xA@ZH77EBJF>riQjS?}nD7Xj)WBrVwWX z&1dus<6d%Ntaj^67}RnnCTbwyCU+EX=P;?01gF|_TVxWU)$Maw(L7qF)1Wt6rW2sU zgb}!JB9HWupwZ(B$IE%WjDI1Pl^4buNc%SPj8JY(p$5V&$op_9@8p?lxj!6NoLOrq zHrfsatu>ctE_oNZO2CDe4v7csgj+h^eF2k%vpN}`mZQFcy9HOk(aSP~TXI{i`ArJN z^=p#nTi({IEs8nfU^Q207_pp;`PG?_ zvvo&;?pAD4QK+)c#3pTtaGmM9m^uq<(i6AnelT}ZniBM^a69a%T~h5Z^v0dS`I5Wd zUpxk1b{Z}OVP81e_{EW@$Y>~JC%&w z%79iWAu0S(n;~TJ8HWItZRd5MYD?yB4R?*5u>HLCkc-=;k}{M`f^J&5C6;1E0rwqp z-Q1X&!54db^4^+|+VIII(ADo>vZc0$WJI^j{vKry0oQXdpF-BxjO&V`jnSCHW&5lF zT3J$Qwq&-1x!|RF?TQ;q@ zx64nJ&w`NbjZ_KsAgSLL@j@p!YU)FzlHXiXJeKTTd>rcl!IoXKcf{Rg-?xw02S6I? z?89wg2<5JQT(oq?yj+ae)^1BYDVK{lf`g8eWuFfmELK#FVyKEisNyFAxs6b5U%M!S zDQcN$ul(`;vxpU51SfsU|B~Z))IXKOWDHmVS|tl1JDD080O_1)D7?~0?c*VJW2O)8 z3s+B336seU&@R&cq_IyjH(%Q*D?_bxHMn5BYX7)rH~o{cNY&NQVAI2|ewoaG{+->c z&a2LPvOa~LKcPg`ze-c9gmiZ9=-D+`x#9axBur-jR^)XFKGKwx-nE8}*|c>vF%drNONb3Zt0H+w zpi3xoKJyAIquI(lQxa!pFo&u^&o?w7m6TiS299+`tJ(IPfyqJmmOogn z(qU+~773S!y`ih?py`UD3bW9V6x>AD&BFHv_9mf1&}v$rJ^mAyD&Ci)Knavh{7CZV42EgU> z)9M>m%Bk?k_W}VdWc#a!{a!oogQK9dNw>Nx^MJWLgfl_BeX0^i$_C<+%vkd@rtf6* zEH_2VBSj_&sxt5ij-2*(^Z8rJJ6RHU1*ERm1mGzAaq+u&?fr71?Sv0ihK6Tq1klI0 zNm-$3BM&Ho9;Z~1(ef6?&(1a1W0aHiFOr1M)RPn>{enb|}oP<92bHKWS zY=QdjyO0D^6=t;}RTDd3TSAv|dF%$cSFO4X1ry>+*Qsi$#}tvbgxb1}P88fS92fGM za>tWQ#txezWiA!gpD>EDTCQ)AGE{QywCwszo2%*5<&825ID-FBR^Ih6qRaRZcd}!P zIW5hH2{nZFC|<6gC0UI1(eoH7S#X}lI@$?DeLJCkK5>oyc7gR0ux(@>ddC3IxB7`X zCO_*gky^w$5}VdO^ecj-mnT!hm8js0H&MfDh^^1j!vc?X2&aLR+04g=dW7g=fD@{@ zC;J^9bIlWdk9|_19BoNO(cpVsUX5}7q_|Pg_b6oLu+c+14Bo2Qq9WGFeiiJ-6bO-! zwSg1O1>KH>I;CJ%FP&l>38x5@1ET^oO20&jP?htaZMBK|#P^-41ap2fnT!XzE?B62 zub#K&B<>$K)z=7-2>agMPA_2S55rqm+w?7%uEBuutbOCG-OEy*J-s+_+{6kj-w8vO zjyz?DKh9jZYNAjiD$z9=#HZ{DOsw4ci@n4{Z{HbC z$IKOvWSzeP2jwYHVYdo<3n;`mRTcVc7fgu#qz>m+JX=F9 zN^)q`9hlkYzA2El#1B8Qfx%uBg(t&LW$|-Lzjv+V?3^5TrD>4Gk*!%fZil#O<;AGL z#EW$}+`pvO^(4V|tV~a|a}6+j=5sG?N&d;$e~)+HyxEIlZFiYj+#*+A49pZJz8%!M zgf!HgWpn2~78wgEh}d%$9sp~U&M=TF{sse*Bx)h7 zFX;9xCW8Io6zP!2j4%r;x2Rl}i2hNwDgjUpD8vs7c^>Mxz2Wx4>$=4<@iqT|(8uvs z{djt*I=JHbJOD*#+43fI{u$QWfZONL{N8kHkL!x+0Rq{j77!wid7S<@OPA?YU!i3?P82FjpeJWV2rgWWqt!sG->s!VLI#t$9G97*j#}|6BJ1yd)M;#ZbEr-z%(=PP#%Rf4-M%1-4VFg zHYile4_xQ(Upqc89ycN9<3Li`q#h($v7eh?fpw}U_j6O3q~@;mhc_f=VLd?WuB^}O zTk`N%oD4UUNtJ4Kb|dlMCW3o*(-)YlzEpY6$?Y;&m&V&1Pw9tnS&~;Yw$>dM%4dM$ zsA(>!v{d8r%BNu#wL@?SXKCux?ykZbXxG0%Lz=O1NC@I4%pRT3N{Da-6k zl1^N9(VtSA4|AQ3DU7_e3Yd>#Rl`1kJ_SDY`HDd-n10F#I~4EQd0c8<(qC#d(V%p( z;Bs=AIW@K9T~awWsW&Wiq@hf;4Wy|}pNSP(5_m-*+Rae0$zI8|4gZD=-nZ|M#OAvk z;3l#Vt-X+}C0Yh|bh0(RDvjC&VoQ@=KPh~p^JfSUI$DBWXSzy*A*8lqZzz�K6j~v{@bql0+XMb z@_KF7n6~CI>m(wJxAYoP@IjT6RotH*MRuH2Vs7HAfMMT_>KAA0Q1gAyrYrF#Zs@Hh ziufTxH@8npXy?ZlW5}++m+Y;64;eCq^!yk(*hJjtY|o=vw`7cPlH-$q&8@1k-H>eb%}dVP<;L|c@x=7>gx6pVY9Qja#yV+;6gSXm!v;o3m zlNpU?>FEaGbCg6g;&o&n4wS9l6r<;Cq)XU9d@)M$n${ogB(a<=Gz90!cLX~ZT4OtU zlkA>LOM_Z&(Qb>pxx8NIo;Fz!$40~?3d#9F1&ZiPL3tU%3XwTa8DSACVj){45Ey%` zkm^&iS3nqviy43xN#GJ}6#YgRCog9P)dWt z_7nk=SIR^8W1b?xiSYrmyvT}}QUhoAcnZNl0u8DCUX(dS&K7arzY(7nk)@F&Q-Kq& z7sa))_dbaM1xLMkoK+#_PGm=f^fpM`5}v#&=&k)Z-+{FX5bxPgsIL=rdqB+G%)?kV z7y4{D9gcPX;544-X5gqj$OOw(21nxv9RrnAp8*|w_~EoZ_*`ppf9Lk?G2Y|2w_%EM zVg@#E`zHuY*c8iG}wq!fvz{^G(WHR z^z%{du$%tnE5d)Mk|XKij=_ZV`~5Zo9X5UQ3uo2!VdhZt@P@}_VH@?kXW!SW@@BHqCQShW);>R2NCsGYUnZZ5GeR!W7IW&$E`7 z@Q2oAjJ9w38C;hQ0o9T3SboDK5En!Blrv8DYv zVX_%v&1Q9ofoY0-qrt={YY(FsmIqVGRX;5jgEF=ZCC_57$wW{h(p1nuv3vgrXjl{O z$RV8Ug=GFANc}17KnpuLXq&S%#8~%rGs+AelbAxzlzp6x$#egX_C!O}kT@gEm?m}h z7v5cbc7s}caRexH|FEUaJ zclVo#GpwMK5{)pGXDBQ-pX`tQ6!?`qTwK41!?f7!s4pP)R$UB;Z`QUyzqA|iESA}MRh^=M)aJYF48YUf6eA(i+j>n-; zOs%2>hXl4t8RfjS(zDr&rBF~d8T)pKQAF~|PvY=}1?O~rYa&1BCaA1iv#D?roxbsu z>PwJI(37pt!?w6vlUESS8;xALtozB$%&7Zu!c!Amr=_SHJjy{}Yl=TXb3n?Wbp_%mo8ut+@{=A*p%;*cvg$ZWIO8%{NPjNDxma}9J8)lRFV3BGShT@bMIpJD0 z!5cy*^n7)QW)U)`Am!4r0*&hwOl`kurp;>%YWFRPA5_iU6~6Ycws7~Do&@?CIp11R zieX+ae+_z<4cn364(+~j4=$GG2M`;K4FB5PY*OmryswI%?h^Pe9q0z+UJO%+yjjS2 zjk;MWyVi3#Z^Bb$n@zRDNKB*R(x6{}gvzd4h%}lZvm0PR!jgfmH#lx2plRz1>zKY{qQ58M#D{M#1erVsi9kB0)qLQgr@2C_3s(^Uol@L=Q<+}j8&d9mIR8oL}B)YIP1O}DNokN=H8nyBD>@UsYo$oB&SD9?r9N66ySuw)xH%#rI zRDh>Pb&`W@K&V~Q6H<$;Nz!K&z$T!6bRhGn_k5{*bOHy5GD&)woO#ogWRB$c1Dg0z z{hVi*%7jJA8UfT-xkN^h4?qR)sO#V3_dLZ}0-R=w7;bMur%yr=qwQzz4Q%fOq5si- zMUA$>{_4Q&PaK3Vim~frr!q1%>rM!X zwSOQ*`l8Cfjm1`5tA7|gq6TzpM!1)NLQta+ZgRi^8=_N()bmTA7c(66o(!e=#I(Gj z9*^ZZOad^8rwIj~pAD_&C-G6}$qBpunpZWd-hwFflJkS!1J>|1T*e9B`*}9r{UuEi z+JaMCL*Mv}F=M5R(S#B;AgqF^Y0Qd*sbSQTKHI3FB;Yl?YYc(8a^v-MQf zRf>bKYQ4|Yx$2qb&HRaX50`*J@!0Jr(r#XQuev;^X zj%hW!!tVuL6zbH{KJ-4e?IF7yh6#R`gZ2F4ezHaKBBId|QRVagfXqajqGQ(3=Ag{v z>Acu*@DoLkfLd)_c{Ygv9X%EDL=NQC7-6GPovO^I5dM z;H$mxoRSK>b*y$#m$i%DT@?1yd7vn6zPFk3?ljvhz6K>n*w?0r-AszyP5VrB67Xif zqJ28=z)V}ObWp=pnQs0o6*A3@DY1^kV?u!S>_g~sf@&h@ffFa5at{)GHX*@^t(P zNNzbwHaBBYV;aR{hhL^h^P|3|vb=a9TfRK9h-O3mQ$Bd+GFH37rc{sYn>tWUERTJ7 z$1s!W37i(mMwwhR1s3}e7^;Cl3lBj#aRbhJ!Pl@YtRFpduZ$d0q z0ewQt{5wqOO;|5%1jP|j`js{7ENi4xvNhJLx1~3pd&WW&aC_iFaK1+J_0bWrOy1`E zf8;@027jEBMrc?7B5!EQ&?4hiiIk(tNE!JO5%X~KR)#gR;r31Sn_{lAarTw=tO&M7 z+lNzxATV@6?ub;ED9YNtZ%9NvmN774{jL{_L_(eCD3VQqI1w_^Ai%IfKJ@Y)@|A z_4+Imn$!B}!1)jHZ5yi7kk6s{yGbSh)7!Tv)gy}Yx-%p#T~-ej)vNjI5!$J_1K0!L zYa5kzE*WMI$mqyp2efcU*b8J1{P6v#5zxhd#F$@M+oUD6Y2mV<@?F-TlJS1Sup)KB zSY3IO8JdoDLBqt-ogdp(f)(KJAo~`DG><7e4Z&6sc8$3?p?ux$3m)0+?^pnbda6?h zH|i*p>u-V0;)r*=ZH~>;1SfU2-3*t@wp`(F$!4<1n*qnihTg+-!G_0X(w^;KXFCmt zeLmBpgk67&YDD4BxjldOc}S42@ydi}#0K;S9Z7B8Y@N zsz{-yHkgrl=#AXrcORalW)2>KC~s~(`NmZ3<`g~1a$1=#uzt14Y!7@LW70eJdN<*Y z^oL0i+sIj=7RW>M+YjJ*4*Ii*u=r56D$kswxuzY_=ZiHBL1_}@o}|ZlyS8MGb;_*l zr+pcmAo5Pw^{`tVmR?iHQY=TQE_NCTt03YFpd)%2A1h~GV{3Wgp5x{<&7Yqc@cvB$ z+g*ygYzNU4Yss3+&U}yl=*oiYk>h1W>|pEnW`1jB4a7a%t8am&kt^Nq=pPg5;-SE` zbpsmFZ0=7EB3?5KS1?fzysDFUInnv3=S)ds7WYtESq4%SkZBi2?6!n*bc9|pDOwo0=(yle-pI>6t0<4-+ka0IOG6;qQFk6qGdIC!)Ih9=-1R*_ z?z~ZXbOk)1h}@>5uUjl|)yn!_y<>R}KDl`u$(SKXVfQ%#12XI9=Oh@P#o??dl~-D)Dnh0w*4sF*g{UL&8c23sBaH8IPE zYW4}9*KxcH-w_MxXA0-7#l|s)P`st(j>-v+mhNfMR_&XG#x)Y0xDR%%TX4aqTta;gV6)&)x*4;DFH^R-j z5t`AZWsjw~8^x;_;ghtqa62B`-`-}4E!XnSs|_o^n9jyfjaCqEsuY*IA2?JeyuoYyv*TD5Jm zo*W7b3!|9Sx8$}m zI``xf>t#-(rNooy#OaERxe^-~%rzJDR5MhqjXf_v<37tQ6rF^eM4b#DFVFW@aE-mu z)*RJk&+his*6)7O=0&yOJT2l#bsMjy0*xWL+e_(XeW%@tlRFGarHD#!jTK8A82#S6 zjzS)O%xL*1LGc*zolHq7PJN_DMV0>?Ata%J5Oh?}@E&}7RYg^Lwz)5~YdL+Y1DhO6 zloq#$GBE#k;EVZ!9u!^lTv9`qZ9R`fZr_~LUPC7>1i`Oo=xYFRr*9aO57jgSh;s_@c7HQ@I47S7M6As<&O08-}>3 zL&8jAXk^+{( zPWRsVYa#?C+5!9&^|?9Lw-xjlvx3FM)z(+T21;fz$?&&1yoPyd(}?=TCLO~)J5!~P z2#=E5#s)9fGKIEwsnnEX7RQ_2LBWS|J6TV0bIR|`4NQ0`%;hGg5%Y{xxp{)ONb@8$ zdP3&!qi!dwtBZ@TB6GL44TwLXf47wu*s98BI@t5?<$|3Yu7)Tm+eL>eX~=0znDkKfG#B5?T`dq- z-pQpjAFPD7O|Y`Eu*}X|qS{ERlJT`rjaZMzOk!+es4&=v4W}?GsnU+xn_pa@ve5W? z|Az(BOx+9(zb;xoGW=Hr#xQV6i_=SWD7ji{>RHS%tFrV7us|)ckWgx3W%(bP zN5obHN0xztWk_(X)p-YkjM3dfRKlvt23?gO9pPz-O?pejC5@GEM-dTbYV~>;n!omK z%+s4!1t}$$7u)L_QEDi{_nGS(nf{QGHoqLE-I?o`)k2^V;nJc36x7d34_}w%>SZX# z5~X?)w560SLDkjRLNVeBMdZ_BxDQ0qk%5QfcIj9-yj?6H4D|Kw%?Y_@n;E)F3`Kk9 zZ+{znHzgFm-Uz!=7v(G=_|a*8(}F4U>Ig*bNZ2D2o)msRtq9kI^&&aUWPP+#76Z4FW$N1Vv_SJkYcnx1{$h z`cXq~niCpiFq3Fot@~Sv8BnkGC<2muvdL7XNBxzUR_(wT z@8}4m1`yHoKLssd_oBnmt;nFE4v8t4`X?OFq6Wrkh{j!=7IyocY$#0Ox6HpG|? z8_F}egjLp9Jfiv!3$D+}m^SG=a@(6Re&_5lRohYS8DpPG8k)P7AMwRAosbO0=y{s! zpVj{SWW+D$Bf5w~+k4V`#L(TN8loknAUCI=75nWN4laXF8?`|9L2lj&qppr|{;OO& z>{DsLX_apO58Y|+vQ}%##duKvIwBW+c!dQnHL|Jd&p64@1t$HL&>5VgO`67E@gHtN zD3lAe8J!636oz}G4h%FY#fIc+xavDfLYC>`jy0;XP-&_YG;}edz#AJ6v{IA29W)2E z=qOf1;)I%^1reJfKQ0d6aspJfrAHdVg3;~v559UT{#f(B@bdBVg$?J@9vd28A zc#iklTydTI9kry5^0)d7Xn?9W+16AH@Hl5dalb@Ek-hlkOzq5NbMjaxOAjKW?8B7a zdI5V0uTlNY0Q zRb_OciLc!4unO>LQ@$2DR-L`q) zqj9DsJ0NBSp9}4(ug-3JI=$oJA2O)npG&7IxQ0?DB{`Knt;Sk_j|ASeCykyz0H!Td z_rC+UD4k!qPN<$TPPyhhE7RZ|!No`0#x^>uI?o?GA-yvm1P8W^J+%ak-Y=@Y&a_bs z7LD_lo@Nq^jWU3|7KfB?>ptSH9#S^%I)-ZgOrKd%_T@hG(Hv@C0~Tv=?Bq-dvh_{(UZmF8Oi`8PoB!?@kWO3k|13KK0= z$wSc9L~U~WRz88IMB`Qw@38E~GyCUcd=~l9>S&MZezGCZ+?RN}DJppL2Z#rkd`ih` z{VHLy1ZZJNA@LmQ9ENtRcgQb;11Fe|RM24~>!uIC%>qOc}^Zm$7E{5Vv>izY;2{tNU{d5qh{VOjC23l0T zkeV{cF?9TQQ&^B+h-%+ok!4m@AipD82+^Dez*mrq5F(fcihwYSlP?PE03{L>n^kl9 zA>x0>7lHBqjs#cz4RJ@fj81^i@BLQ@$q%f_4j%w6idb91|3qr|Z}oVlFHPjX2LI4f z82-Pi$MgM9w3Po5l)}hB|5rW!i;?nILY|eG_5T+qKEf5W6Cj76>TAPYQs1SXB>1-x+*;1hN?_Wy!6jD^?9PQe4!$1v*I8d6+vr7}|J zT<`R>dNA)?VOsleNb|;_emn9giz@n{IXpYCTt7M`>dJggsae>m?@FNl*vfM5v*3#K z?9{e?X}|cBG@!@yN%!v55&3kwacbi_a%Frcy@&s&=%c~y$?}fFle-JH?~ljidqt`7 zn4#nFP|0NQ#OS3VkQ00dGHe@T8n!pQuE8-cOmEEi>v!19+HLsAU3%>}( zyH`aWAWWSI7^D&YAc>;!Lhu-y;Hj(u~EBmo;UxBX+!lUOnYX3)=_sI{`oTX z;CQ>`H-Ry@biAQjfw(vp+NI9xJ^{Lw4$!6*2B5x;5O!oAa_XS5&l&8hW-A>LD#@C zsTVZ2{?3qwEMQKlFH*^h>g=OqIF|_(-gv(EnlF?M9S$0ItVc*+Hqv#lfhH5lT)#qB zi~>Au+N@mneHJ}uVHp#agmF<9UtSe`aIA-iUK9<9nO9C9=#KctnnsxnFuv5Y-6O?@ z&e44$%BmxBphFW3tzY=|MqPJ}uwC%_(5>*+WSfqI#%1iGc%7O43V0j9q5>AWxgsf- zNmKnF1`7td|MQMgaJ4a{7P7W-pq6*kbNG9rVCQJ~&s9EMd&7TH{+|Yi|B>iIOZNpp z`VXYbU-HTSPA2(FC;59YevKgirm_4NedXWtuTQ9d)9JnlDF0ml5BvWhpZtT1`J4Vv zng2;-VPeH&Vr2YpoXmfejrX_A|Gf?W*7w(riHQ~JKg;{SKA-*9*Lz*?xePT5#ki&d)Ktj$e8Sqs&+%Vh~2>BAawI$y5I z=!Z+Zt48M5l}}{(8${%b@ktslJVs8W(L4=q z*;`fsGPZR-Bc_CN(b4tx_VL0+)aE{!&iWTPlR|Irl!3m+#B)2$q8qx*{3CQlYN`f_ zFB2bR+;a`7H;KKo8hxw?LRSkT!V;t9Yz7t=sTZ3-z|moBF~eUmtA0~TWb+Fo5d{k? z@Y?k;e-Ri!krK*q2#>#gIWL8>$gJaeI4!aZE#R&Ux;PYb*jh2uIlMrw2^&D+3A%=h z8R~)mDDDKrFLHrX4xJYh1N2!_)EV{QA8`EGpL^3kx8L!bPpvfnae0}?;yU(CyVngI z--5pP+ZXnmgMzGpt3$&4oXW&PQ-~dsbQBq=pvI1D?A;^*9 zly-IXVXRh5i6JwQyspi7sW{sPrG1`mwV^MRcil{aF~Xa!_7lf&^Mn ztg)zs!dhUgo3!?%2mq;RbfaJ-u1+`^iOyWP)!_jpqi_ zG9624K+=NaK9{98a#V#lF)u7)PUQLnXkuT?PQG!dh-XNr!GH|9jX_?9u_lR@+aP&F zy`~oayYUhsF(e%+dhLnLFQxW`_Av`K5`q`od~$ZHuX( zKL6JiiJ{)bEKxX`rd*v(58#wl(RVcLR+VT#kp4Z8tEx|LHxjHvs!X;l9x7_!fgCLs z!Ahi7(65Ac*ZNvGm25N+Y)Hma(AB?-HXZpF-w#}kny!*iKR`@A0&48QbI86=pC}?N z_*5VAT(TZ2l8Y{_Dh2EgTI|%~TcG}QT`SfY^THcH-z{~0dst$Np4yE5z)>GgJb9>k z4*p8B=l{{%TZcvUt$&~vohpiggpz{9FfcP9A>CbqbR*p*AOcd-At4|Q(%m85Esd0P zNjKa*qkfO#Ip>~x|N8Mf!|Zuyt#`foS!*wN_cBO)_>R5)W~^(T3y#19YpR`Svt zWGo}NY;Wk%At`IuJM&$_?zM5i9e$XOOgVc- zBDtrgkBz0fdGhIDqkoHi0jo;H@aATq@sdQr&9$eWA!1^$C=6*tyZ48w^fw(_+`uG4 z5htsJA-cD|M6irnD5rXPBqw?wVBC(!{Sa`$uo;ix(~7Hm^8zi{S6I*C){yl{a(v}Y zT@Fy+5yUZ<1S9}2Ha@f1pyXIOfq=UCg8X6=<`Z7i!% zgF{S%S}pjo7K69!DhG6=u7<8WD(>wn8^wLJjfzWf9BBFapMUe)Q0;%N*ME&J*5=Oq(%4GCp=Qqt*`qb3 z@qsfkV(kWw*hw64-4mQ)&w$Rxk_{3x4{yA7At^L!`5PIJq?O(8ur%3S2$Ac2D6zzQ zw(8vSS}Kz5`t^Rhcft*ayU&`eNrM?j>Qi1cVnVzaZ3rL|n!9!$LXU12qf67hY-AVc zDGPjk!|~ojuI444>)(Red)-e^_mz1+w`iPt3s|Rtns4%oS(T+3IWFbIZ3u3gd)W>= z#g?NwlTv-aTQ*Xb&5U{$lBjZrAR5(+T$>D{LU4*B{GFjmcbkD;bDVQ-s*yMON9;ge$r6R)Z9G|S+j;STvE1KTpNOjFN-(l zCs&1Q_(Ei&WK*c0S>3mza9)^s4 zC3#Bf429F(XP?{*Pk>4wk@xnN1t+}^7fLq4&Wbb<%#|`vsl+$*7LL#5Vk^r>yG-m@ zpp}%aJ$Mz#H?Va5EMUVv4nAHJwAeTOnlcnKW5HF#iZ5E-xqUrt#$rt#R?6$kM-F-1 zL*g31SgqFBy0+*U{l3b)9%ItIy>y7Eq9=lGC_yN7N(9&|#m9%Z|{eAt^L$ii5=^ zIim8$s0}P@Q!9?A;u&0$EG_UQhV5y zT(zI<9Z`;mr(r#XB%7+vnO&nT?3RX>U#IG1pGiMCeP~z#n?mY$EaW0HRoSFtqX5OM zi&Uo7%^P5pztfBDH!TKE1yb-2XAzNI(0A0RMtMR!%?{JW`S882m{`dEV}rZw{yyxV!|N za4u3d>0m4I*m@T$b9B2(7rWeSSn^X!Xk2EXqhrATz_Ts`?#p0B>gh4TInwVb**TP) z*}B2T6L+nten039{>!UWq+5Z;q0t0_q@-JP@@p)tw?V>`&H)cQzINBt<%Foj)#mZB zTrV{KtOI%-e_W3gE`_yo%9$dsymG{7=Tc05B?JBYccWUG6W;^6i;@1RvkK=FcSFn? z?Dre?o4w$)XfOP1UE;}D$BL8ojss^}8%y2Ef{3?cDS0*X#b%rug905JMrZxiI19y& zZ!#nWYra&QZjn)TGCXjbAz^%rX{v#{$H4BG7+TRkHLgm=^@*-#SE8T9ZA|+x2=v!k7|3g!%X)tAtxBhXCV^L#z@K0 zKvekjrrD=mxnJ!~$v?eXXW6~-qQqEuGOnWLpkF?LC7a{y3wZr4m&>aK*-|r}%6S68 z$<-H}P_~t6XdtI_JG(I}eM(2N>54P(P#fW*;78l?`3)Q9-=gIB`LlvrfZK`j^+p{U z1B@s!zRbN`?2IA^#rRzrWwc|=`p{bj%hYbN6pU)uW^UCLJikUR+q7K-G9IHgk{Vme zxB)yr7Fn1uCmCv}P0dYlQ*0-Khfc0%2cJOtp6Ek$-7BAHF(P@qjYTHSj4GAIVpZ&> zcq>hp9?;}e&Mo+K3w6wXI>z2_8(a*XvK(ZQfB$-}U{VyepE(s%x^Cj6*4}Ly^R|-h zbl`*sb9f33$4{2Vom*Fq01ZbZMXl?_7lqOeLn91Uv|Pm(JJ~QZvb2uUHzB;PZTGz< z^SZJ2Od~guW8jY$Fk4nT$s@!ro3Vu&+wpQW8Cvt^6Miu5Ez>+S!`oCIV;3SUAhd~Y zJS(z#Q=-)q6;{!YFN3~5HMMWUYMC?-+pmk| zSbN*uDd76{5xb;=fR+|(h@UhbWpy$uSL`N>#%%gsCZ#^=kE zwRUs6o4(6a4mWw|c_7H(;X#!p#sOcg`jY5qR(S%$m^^}q>+DM5Ec!7X23(k?97r8c zW&QBk?@mQV6Yaf`s9(XOKo>jUJM#?K$QfjGEA^x`U|pqFR|r)dm9TUArsNeQ?yTux|Wfl zle6IkqRA!nBNVlYk4{8e%bx9etCYFsO@D!^J1pB#aTlg^$*N3M*z?gji9;g&SEGPUjGS!FFs z;^5&=a#g`wT0B~V_AKw--hglxTF~qIiKC<$yrjU0kja##cv0b$)4er1&T3w)~ zx1v*Le))t*?vqLYI$3+lH6kCvbwS>&WJQ%uqvCt&<7;oK+q=E@El$*)KGhYB+AJk0 zEflC6CN3^ygQ(X+!g6n>r1NTrC2p4B8g*s8x^KaHsQ#XtVUoQ?Y@uknK)S4`DhftG zfht&<-05CclJS0c*f(KAS4&b-@2N+s?&QOKTW9aF3bPgGg{d+lZ~sAWB;_KTh_2G7 zuxf=V;(l+M>^>fELmTgr0=6Yb1sa{$q6dvJr(cS3lhvY*A7x&h_EIn3Yj++uy*TPw ze6N@ro!?VH+pSWQ^=UJ4 z4ik{IZ$@ZZ$eT0DcQbFci*0NU$cn5|oG>=Cvb|f&xl_60tt1>K;nnOdJF8^m;Y<+Jj;YHQ^yE=1g;o?y3(t%(%<9NAJ@QX)9cacdcm*RjjpcxZL@blGUD#KCN@l{>vOtD)v^54cV2yclcWnAefxr{+ zDh8}OXFqdqZLR;lM;IGsRRUdd$z8nXAUhh{ErD3^reaUIA?DEH&$sU-+_-kNBAGNQ z@rXSp3^E$x*BC#fa`NpfK`pYjWP`(u0UtzSU-CUs9G6eRlIQ2;>sBwp$Rc~ebM>9v zO`KXNN%j+oU`sXnx_o5MfJzGHoa;&5nGLACf_(KmRdw1JGiGQRHf5Sgljn_SIBYYf zR1?!4w6W%_{-ZBGWd4_X7Om7qd7S6MD){17QC8xFUB7~xowi%Ef zziG62j*juhK(BV$_sb!=YP9C$d@$>}zbN|ldUKWLBqRPtPP#$WYDJV4?Jk&Lqe`tm zDI+k$tO9AT)$MIIdAHduEe>>LS;Li(|z`Ai8VqV3S?fkLyeuIlZ8;Vu&`72g-$Zw~pv9k@tI zQlC%@&8ma#rDyt3z7a$qe{#O(_(5nAId0QDhBTMjM)@P>^rwu{(bYTIOV`nb-#<^) z#Ms%!bX4GKm|^aTOwf=0;HsBk!1#@|7c$0+q!K6787Cp7FM1!U4O?)8#013{rTUW{ zEWOz)w6>#8<&HutWeDZ>ar!Pv=;V}Ah}jYm{WYO@=J{elLGRf?UC(CCk&I+nKIjRN zWnF1P^Ky;eZl+=cClZBjhS~Z_pb|EX6J`sZM-8pY!)`5Wk#LEo7}B1gVM3-n0#!4i zj6TMhpysNn8=%|Xl<7>|Y&8+~l1>R(nW@OxOh*g!tvChSw-|FdQBvQZ4DaH_n{e)u z?p)u{9v-c}o0mo}8IK=S)Gc%_w{>krjB}5J__)MB>J*hiE!W;H-Ny~q#4bh6>|ksg zyTA!9rF^Zu@IL+Y3&tB;BWYoluUu!l14F>W%uFm#p^^tyF*%jp??sL_Tt6sb1y6OT)wVY5|WM42A#?cQ(>Tq3IAE=b3MgBKDGgTB!p`%|5=BN#iOmlrXDA z(xp_2qG5G~?RxLM*GE^Wv0u|)Awijry>be262JPk{LhD8LyO&7q0Dn*d(v9v zbMjutyYqSarmKUaB7JR4G|Nk&21V~y7H~CdO4L}hYjn4FJD2CWPghPgYQCr^GS%*H zXJ*H{w3}>mORv~WIUh(@>}QnX2^Yy<$MdFoE|Dht4Ww+Xc2Du;&T~%@H%O*druFoI z8$U`GHs-w@izBc@-uLey9EPo+Z!=eLhPQ*J6XLxbi5Ir&wiw@-P}!IHd-1btc&Trh zhj~>xJn!MXzaA*GDfQtx9@dGSjF<-lcQ01ht^LuC>)>u)6r4vm*KpWz((LWyyw2E3 z*_(MIy=aIT$DazKIQC3dTYk}#&8hg9dc)AWBA8fbG|f5i34h+FI!Q2Icvw1hNF^0l zrRjE;v|bZd-tBsI=_iJ^@$y#jA}A5@o7zbmdMf_Fvkz#GrAm}13Kx8l8b9U{WYMQJ z3=vwI&jh)5mF~aYmYi~aW!xtOKS%K^=U zH!yjr_ZkKpGREw1h_p7`fD^aQO4w|p#gkpaL{e*F*gD(jgCE|zMH}!qx|boQO|hxi z(Ml~&B!lt3eNyn8{T-vWVM4LD=B>s|S97Q3fajiE&w(Z*S@mCAQg5N_)x6qw?G2JW z#)pEsox3BFGaw8(rRi%sq9neFcmrwVQCMgPN##~sk11UEL&V)AyH8#{$>W5D5v+KH z!lH&zs7QbZu3K)4lr4}yFo_6opJIJM3Y;&SR?|>UiIaq#%guFhJ*-gBmjYwW7C*De zUa6t```EUy3p<|)Ss+DYf_0gl#{-d+hs6Cfu?4O{k__f&J)<^DaXy)(DC-o)y`0y{ z?#b#E?kYK_9+|J?ZpNMs1P0EVL@F{V9Bq_r-SMkB>(mYnT+@oBayjo%)aP#4WSojG zy}0{6PPis&Y_q%Z$d@G@AHpPB6p6<+!LW|aVws`;#P$R#)=ZEMNE`5Zl& zJA!&5I_Q($dOJ2ikp>UFx3H@IWg$@v8$I%3r_m#bhPQvI{3x5T=8MjVqS%7ie8YEH zO@ns}?%7ninPJ?|QcjGPar2#%{z%xT!jupiwzsDyO_>deDA4y2)w~&bO5xeA;(EL` zgreA~UY2fq5%QGMwb}@OqV9AHqagz&M@@?PsL)W5)wx-pjMFl_tUZS*=*^o~@-7Ke z5mR@0>`aU?qP&O3iQ5OZKll+Z-sD1cnLfK0RhPJU);r@h=OT2hSxcf-M01?b9&bUk zNMhXKrMmb)Ma8^|D&AP#JnP^ToXf>^OJ2mwu|N20>PCQ#3DGiH^+3GK2g?)NvoF#I zqlJ-QYtBRC!$DM8T%wC_{AFD=D@W5nM%zK&_CB%+t#HF2IW3ErjiwMyEwLUgHb4EUJlykY zJ&iDW^XRjzqQAm$k^6V_<{awflVcB5oNCnD zJ^J51t-bpuD@6NxMnlGGpO4`AVN^ld^jL%I;d;iD+Sar1JhjnKo(ncq(T{}45oyG( zpQ!fqR*nXpGKTN=xRBYja#3Yd-7~Vn`M4=NN4x{wy6UN5L=H*nTJEO?)elAP=}j0W zL;dv>MAbnnT_>uH8*gm!Z>VTke-6WtsGQEju&pFryVi%)h@`J#-j39wYVL=TNWYYF zSd4cOV(qnP5MNy+>4ou0rA}5EI8+>MY%pfoJo;LEQjr$keU!4+fL9wiAI3w(o4W_p zlpGfC00nH1IyA^J8+I9qtj}Lz}w^^x+3H3i9)*6$ZQh7`mGg0fbeFAlfP=4=F zeb@-jz)Hu#>W6-!79~<*7!t~j*kemk9wn41lo=W!)apvxHCMp_ad6&R%SNkm_Ai(nc_E2X%WT&RTA4#rC6w>nI#XzjoxB2 zXg+C_)^}@buyU`vcVLLf$kRU@d| zgcsu*r7g?wfbMbEoTU!M(Gc-5r$xqd16hJy$I^b)>AvW3UPro3J*CAJT%)J!Zj0>Z zku~U@#E;?*Tx9pdLbwX%A{2bSbc$k?q|JWqF1#q4^&~Vsm5?>moIK^9Sz*|S8c_jT z4S8LZoqU71u1HQxFPP>(q$X z(KG#pDmP6fq=k5VR-s&X^QVR@ zVk;&A@xHesp7Udg_+8&l!ljJY4O?ZEe`48a#j~kaudDG@O4xr2VP?2XCC(4->+@*TWG!J<|1u9bG(wd9EjPC!^lKMgkL3) z`ZTtkPd!;CBjXz#j&{JKg|*{^S>nL5dS>>R10CF0d*y+`|yh)Zc8l`NKCn`~#1DzKQ7Af0wUkglT0sH?FiHgKwDK7s8s z_<_O8=XPhqw%p65=Uw{9mf2jrmThk;3Vh81{HK@g@U$Q{6cRmt;^tNFRcT_viB_mL)G4zOONet`&w^y6?HTSK2w9f%1=R99A7 zUhjRJJD;ALuqSJ!g?jHM5Qf)TUkPP(2~F)u2}OP3xP3Lf`$4s?7Qym&@uqw(3Fm#m zXla$PN~@mM9KI)mEO{0#*WW_iOIbb-IOTe=Fm5j~qzr0xPeRwK1hU666gCcdj3?d) zatCyT9nZwMM~vcc`B}9Y6-Wj@m`j`;WmU5n3nXrx)z^*KPjY2d{;(=|U*2ME11O;=VpX%m}7XzXIL&IvJGi{`|WdH_4lI3n%hyY zzJ#zSYZ6m>80)VjwY`tRGE<2s=u6Fcz4|Ql_^Cln*Vm(?;W%AKt+UXB)E)wPBl8~H zI?OF$Qpf7C@Rov??vthpF|qWCi+h+oGXkdlOU%^|DSM`;t)Cc`6uwL$AY;ud5V4BF zn4s@1pU%QKUwbpG`kfa4)I$Jt(4Xg-k zM|wQa7e>;6OSbklmobOeixMTkl1%?1&L@zwHj2o|Ha7O74b~*|Nru3_^E)WJ6m3m{ z451vV+^UH}=B?dn3YzOr8`joPM>~;3Cd6NRmf)uGep(yE`HPj z%2|$0RiS|IJs9!~>|SBBF>A-k@vIJ4RqwWesJne^pNY_>T-RTo@N!d6+ZRgcX^R(5 zkX)ezRb}hiEY31X@^4Xm7QrD*N$5>`r-1Is8R&8{v~Y@(ThfyySsNPdDey9B(+Z_& z{$9^Zo0k+7mLWUXI&L~OXrF+pch_+)AH=cYPyz`y2xc^cACzA2QfxV@zQ z23U*R(nLgt>)fBzo@WCkq%Grie9)^L&u(J(+`F@)Vyg)AP%xH>F`fxRyN!Q4bKisa zC0+Aed5i8SgSll<-0}DaGO()+Zj?xTns5tbSIpnR`gm>aXq?318}3P;Xqw`+s7Jd7 z;`!u@SFkAUG))KC?+14e%Q#fZP};09JGXof2}>KW!z}FIyf;m^B1$ML?LiVW)9<{2(^D>=%k_sZm%e-LXFav8G84@St3f@b0(D+Y-R(~ zrasVbM+VqEwB&IXYb|Bs-ybfN6WqDU29-TF^FT^=cv+ z-^p5hZVWNUkx0`R_2%oBKYD`WZui)y=!NEa$LEW&CAHikLmF_vTt=R<@D~{Wg4wVEdsx>Q$@F z90wE5d&O(-7&b>-*5jVD$2ge^FUIXwMv10I?eK}b}dl&nlj4=0Z z2V=a2!)5OvH$d3iFk-9}_bffXscpo+~~0JaKzuN3E;Xf=oMF zKQ3OPAejvB&RLnjd83r4qvdR|deuoAi)-0j%n3c6(er@|{>j$)z;h>>!KyryFYWSq z7fq!aw~K17y?;P0%5gAQ^?2KKdN@>nqkJ*->AG)QJnkrE!M=!gJcnf#^L2NUxp(!i z_8;ZRoC{ zP@gzDH}#Dg+`PP{Bc3H{k2%kvCmv^>xJmN)xS!A&#E1*1Fcz!n#RX;M82YJ#my`BZ z954#htw!)~Xf8H)_*Js+#k$8t86i_iN*X|?j$_oT_5=p3o(?r^B?)FxF5Dkr;DU;t zpV$#Ln7Ce?pu~@MEY%cX`A-k;c0Q(N!xN!Mhh3{PED}L!Nx#1rfcvqw^@-6s>!u7N zet1ClXPg>+dR^X7SFSEO$v8a{ugP`&u8Gul#;mFVW-4SS0=ts$T#l==eKnaK79G-5 zrsVi0wTFn#h&i;IQ6?G6`9hQqY8MXcSqd&BayPR_pN|M^w0!Amu6N~BE;nGi+x-eB z%qKRZmMii^HK?(6hj5Czx+Z0~?C|!i_yx!6?Tyx`&M6uRmoVjb!D|@anUwD^8(nDB zKWQE_fahx^;?8Xz8==mivc6VzOg4RfU}bTuvBPipnsH2EPw`<^(s${8N>x|~cIsO> z#wjw+46If6L*u!)CKvmMAGBYlazU@J zUxO|O6@G<_u?@jkMo#goHe8{Iy4y{$=aliTrzJ@Ls^*@xm`Dt>-gn#~pB@v-_?CjR zRh{K-irD1a_YQBpYJTO$k_WX!_7s;+zo*FV;bc)=z{~pS75`DdC;7edu|!k(mO%ys z<7gi7Oxw(Tnsx3;wn+|$v8pZylwH2~-dY`M$Qnq2;C3^i1D$%S*?jIiPPDzOn_=8C zF;GfsS~GbXbhLQ1a(X*roUXRC%E+X^R{10wwCX(MmE~ISWt6yt!}KJjdP2=?GmlI6 zqBqX6)3Vd5hz-&jogeU|>CEsxdc8ouG6)QB2%!8RZ;kOP%BEG6i+o3-a%8~cgjbf>4WrXzToY>k4p4Lnt1f#9c(gn zr%z6X_rp71SSm?=Qc6;Re8e6UofycP!>tsi*Y2z{oM+-4RLM}$lOA4d*9{1>0+&TxUUT{_E!a!965^K zrbO-FMsw>Tnaa~wMdkGb33euTe!h3$ALFM6AG?}R&Y93FIpaCNoJ)cM8^JG^IawgkyTq5@OM4HAL9p>yH z#*5KT4 zu+_dMXX#M@0p&SAdwxfl9C@~{uR`cS3r{zjmtfMCrDjX7`BYFI*)9BIl-Y(C0{Ip@ z5;GH=*GdG;>64QB)`cujp0RFoz#zCJ1jLh%8Z$Kdt34Z~$RCP3>X3pU6ZoSQHdHwi zLYPh_%_(D`5BKWb1d(TX#_O;(#jVMij;q_5X{lql@v<1%ZxQ;Lrm-tDo-`vP3s~d{ zx$5kEADX!#L+JaC|5!?kx@4vX7LJO}~RYn zG$(6~kLmrw<^qL#dQ(HNKZz;t=VMUX*;Fxx((x+wlenfRzpAd!nJbCRsgLD! z9;>{{(oB9VRZeB#*La4?+0r_X>L){dUT%G(k?^C@{g#NU%ByK|6l1F1`4L(+y>TJd zx2%h#eZM#a-yFASo(hv4b&@DCxoOfJ;<)lf^d-#rfIj zf(Xmf*no21`V_|xC0{f-=zD1C3MJb4@e)Fw*pgE9m+=-oNZ4LWtETWL40kdWAi_JJ zhrOAAKFaN&Cep6#Oc_j5i8vswZdoo%g1voq)L|gDF>)Xh$= zZ94?|Mn*WWSgGU_(V&3WxBG!o71x^Mu8%x29FeVB~F>x*I$f`DDfzkCw|N^mp%AYHBL32`;(cW z&%W%-I~RNl?s5<`he7K3bdrk{MfU6$(rcCeIf7$x{bL`>o>|_cnM$8vcuUv++)zy* z-5FCF>s_!Y;}|BXU4N~}C$Dma0jhDaluoN@*Gb7)S^S2(X*R$p74A6aDl>ZW$9=x##4 z6~$1@_i`Q=$fK&O@mQAC_g4?eAEhTlJ9b?d%(ZSmi1%m+eMZSm2*ddHWa(l@uK1v9 zx-Dl4WoqdWRq$}AJk?W)fOxSuxeqr=RHLAvc-4kGje-vj#i}l+r1ma>(09of;e2q3rW69ICb`ePGLU8NSkEqfnpr>oYv{x zM(kxLq*jNdjo3Wv+|!$Q8(9^h{*!2S6Kc~~uYj|i-klibZC1E!&cw_#+vdJk!LJDz zbt1CQb+1-`%kWUg;OX~q4->IR?DPHy5)RrhbG+QXJF;r}y?rU@1*#&p?X;l7$(nYJ zxveg9nrky(9lomhOjF`Q4R`%3p%Vo&U$NGX%IrL5R z)Q@eGx){opVj63+?#=Yb=F)GHX*E7DF$rkrT9n=dXDZjP6}w#v`23YD)~KUhIUpl- zsEAyiXf-sCN~uVp!Y|V)q@6>!|4U)b32I$e!m4S7&!B^Hh}EzyRYRPVXaNaEqTIcf zYn!ZEWHt%*?9u|RxD2$SVK=MVAx(xHaU*YwZ#<^;Yk7>U_FTB}>QxJmmMtG$<_A6^ z4`$JJS<$G;ES5zyoMQ%+9(lbMw|Jt7vb^asz#Ac5U|nP_e=O^9as9x&0QujQwES~g zI1>|4`0!8t+y8o6IR9VuZ@QL-djI*j@ZYN7fbs?4sA1@TDuaWu0tbZuc3e2*&*Q=w zq5r-P4p9{GXPLy`$AzPr*j={IC?omCx6G3V%Bok+)( zuI^aHxt3QF4l`nJv#(9stnQS@))v{}3x7Gum@>s#U3I2CdjEYZS~Y!MSvn=g@gQ=% zn{M6S>hr^Zo$tNO4%5x&wN6nCjf#F=vV^$+o41C7085^m?ic5s1hCnM6f&E zx6GT%1~*#o+v6yR9OJ*kAZhC_oOu34{(8>6DR-m&(rE!p^ubS&tgVV7v&D>OtGC2l z$t*S*wmf>6G>+5mz_cIq$nQ$Ze2*(Om)b3zedg+Jr$}+XP-eG>Ir2PX5Tf(V=$P=E zLHAU>-~bmliMT5UiTH&;ax0MO?>Tcf-70)5eY@wbQZb9#f_c5UCE}F6H)9l z!b6~EBi5Nw=jYy9f$W4@?wYOh$m|4HWY@U^oT2*U2|{!52iM0Ob~$Fh&nRh9bNxJM2(3pX&g7XL75&aQ$QoxDb!13Y#s(bp?4tR(2hmZS9 z7DPKZ?dSNv__D(J0(7`6aGvmS5Mu!}`1>!}e&~j1i~j5Tf9kD%w*AHP-_%n5b!s>B zU#E66|8{D(0EiS(=?BzNF_D7d&IPW3Vla61A8^G?%KVeY0tgPLp##YA4|4u0C}w9z zln0U`I0KbjQkG`gvbxqU7^L_F802*AtyO>T_FJtkqJ~gPQ(u=sO4Cx;)EcN9LkujV zYh`9*sjX{8%KYOzadBN8Lroqt`xo$HWfoQzdZ4WQXrN7uNoBuk4%~0W&N${@c&o< zfDqM>zsvgD)c=1?Rs?eVF6(c=@qbKKCU|)M9s|I%^H03;n%Mvt1%f|Vz|zW^lm!tl z3}TwVH3ZS`cbG$b`x8BWcKp@v2go7%u^_I2;#Wg!V_kLuegOeSMli5eWa0;2jEs!T zJdBJG7=UvC=0gA)1i(Ci4gvvffD+)}X^ap=TM$1Vzyreiqd$}x#Lo?H2hiaB5IkTY zetvkHfAK@`<>v*)7T`oIMXg`_D6f64q)Z9{(=!1DzRX~HFc|Qh>MyWHm_~*{0sweY zUQ~;1Lmj2*L-@0O6s`KLln0CRk?Rni)_63>X1u{?9u? zAwUr*Z~+0@K!ER|OurQPF>}EcV1XhOU}d6b1!nQz6u6x52rd4L&|qMoe>?+70v7-- z7%(m)^AC&vd(QyE2>{>!p(x-2!Igk|Azwn@UZ^>U;v;T9FZEpcnBx)0~heL z`j_&Exk-lMx0nYp(}VuA_K2AO8UL4OWS}JelJ-o$(jT!R`vDs)2zy^jf50)n?81C0 z8-6+Oe}aR*gNG6FKiC=J55TVf1R*?t0LXyD2=phc{4ad|2{=Ti|101S{C~pQzYjvt z55xY;4^UPJJ#ZuYd+Pokg+HL`M-*OOZvadAOMdtRhu}C0fF+2Z7p@%;4gW|7rvWJn zPZ0!-39tY>#o*q7_W^z(@C%;SKcWzbDxlA=h(uWJ2Tv%R7yR=tUjH7{8Cd|{U;yla zroihLj{RnhA3(;;#C#dZ5YhKD5--oTh#31XpvDXg_bZp+Qw>79|0X>=?|y0mw+0+| zfRBIB5Qq#1A>fOjQx+oxhyw&50aixPe~$5EJOI$()&_XPKmU)G06|zU=jRVg{6Hr} zqW!k+VEOIN=f{-%M=T)bm47ntr*v@Wg!>nfd2kH?f5Rd5R~#TL0JOPG11x}mX+F5G zf%XV3fBOAjlQZxgU~z<1er49LaRKWg`T&^+kHa5Ug15hviGT+X)@S`EUVh-qZ`s4l z2){l0PucT(>;R4aT>Zca|0Q<7z=ZRU#eQ0cpBs?!XMDh={UtpV0^l#)0{>_VXad(3 z9wWcv4p0*ji$Ar8{>;jsaq$DU;rsy{gIfe{{r?zi@c4uNN2{1e@tfMphbGHrz58`$m zeuoY`f}KIsP{-;8DPq+{iU4g=1Xuw}YIxiMj|=2@N zt7)xiY^Dz^P4$6YgCE-n=nRtPx~AOP)`n)LFMbS)nA=TljEzYVzZe1L3WmCNx|RT7 z=;^{Y33RQ1yL~=GD|2H_2R<`x#JU`@Q2<}tGf3K48ylJeBm7wV0{7-K>;(ve@IP#(tvAt*j%E4;Ex#k zg9c+_gztO(K?Az7{M8o>gD3kR?O;qypuhTpVK5-V{%FSt1+iY{2V(-m{^|>C2VNcn zSe8J6WcY(0jEM!lr2c~jv||PE`%fAR;G#=37!#BU$g0cjV1Mz0vHoQP7!!;YK*K-# z0;J3Of>^cxpDC=b#FoD2-*%^3nA>(D4nLxm%-(UP#;S0Y% zWCk-c!WRISX@BVfW@du^-52(kt-vf`@MV6?F!(<1pJM>Dzw~7W=HtsgVg^LM+!shC zV1M&Zeh^@r^Kv_Y_E)SyAizfH<-Sm6=D%n#05C4KgE2DyWj}xhz3ev_fE0hlDd5Y? zy1_uez?a7W0o2PhFeCIYS%5J3yWQV;1Ou}`{*naz=t1oXRn)|Q%v#=4g94QE+HM_mB^7-Y=MfZZj; z?1j!CY^rBQikMq~U3OtU(if!c%up6SC^rns$^->LS@;F`z`%kF1mTCTwSedWaiagf zH-XQwz+A^`psQ_UWn)6h0Op4Zz_sBf(EtDd literal 0 HcmV?d00001 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)