commit 4762a3032c2bcaedbbb72e790543d3a57b67f4d6 Author: duongtd Date: Wed Jul 29 15:45:16 2026 +0700 first commit diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..5bb2364 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,492 @@ +cmake_minimum_required(VERSION 3.0.2) +project(move_base2 VERSION 0.1.0 LANGUAGES CXX) + +# ======================================================== +# Build mode detection +# ======================================================== +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building move_base2 with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building move_base2 with Standalone CMake") +endif() + + +# ======================================================== +# C++ Standard +# ======================================================== +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + + +# ======================================================== +# Common dependencies +# ======================================================== +find_package(Boost REQUIRED COMPONENTS + system + thread + filesystem +) + +find_package(Threads REQUIRED) +find_package(yaml-cpp REQUIRED) + + +# ======================================================== +# Standalone configuration +# ======================================================== +if(NOT BUILDING_WITH_CATKIN) + + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + # Test/ -> src/AMR_T800/pnkx_nav_core/src + set(PNKX_NAV_CORE_SRC_DIR + "${CMAKE_CURRENT_SOURCE_DIR}/../../pnkx_nav_core/src" + ) + + # Test/ -> T800_ws/devel/lib + set(WORKSPACE_DEVEL_LIB_DIR + "${CMAKE_CURRENT_SOURCE_DIR}/../../../../devel/lib" + ) + + file(GLOB STANDALONE_PACKAGE_INCLUDE_DIRS + LIST_DIRECTORIES true + + ${PNKX_NAV_CORE_SRC_DIR}/*/include + ${PNKX_NAV_CORE_SRC_DIR}/*/*/include + ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/include + ${PNKX_NAV_CORE_SRC_DIR}/*/*/*/*/include + ) + + find_package(PCL QUIET COMPONENTS common io) + + set(STANDALONE_INCLUDE_DIRS + ${STANDALONE_PACKAGE_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + + /usr/local/include + ) + + if(PCL_FOUND) + add_definitions(${PCL_DEFINITIONS}) + endif() + + set(PACKAGES_DIR + robot_costmap_2d + robot_nav_2d_utils + robot_cpp + robot_time + robot_xmlrpcpp + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + 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") + endif() + + if(EXISTS ${WORKSPACE_DEVEL_LIB_DIR}) + link_directories(${WORKSPACE_DEVEL_LIB_DIR}) + endif() + + link_directories(/usr/local/lib) + +# ======================================================== +# Catkin configuration +# ======================================================== +else() + + find_package(catkin REQUIRED COMPONENTS + move_base_core + robot_nav_core + robot_costmap_2d + robot_cpp + robot_time + robot_geometry_msgs + robot_std_msgs + robot_nav_msgs + robot_nav_2d_msgs + robot_nav_2d_utils + robot_sensor_msgs + robot_map_msgs + robot_protocol_msgs + robot_xmlrpcpp + + # SensorGateway là chỗ duy nhất trong gói này lọc laser trước khi vào costmap. + laser_filter + + # 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ó. + mission_adapters + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + 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") + endif() + + catkin_package( + INCLUDE_DIRS + include + + LIBRARIES + move_base2_core + move_base2 + move_base2_noop_action_handler + + CATKIN_DEPENDS + move_base_core + robot_nav_core + robot_costmap_2d + robot_cpp + robot_time + robot_geometry_msgs + robot_std_msgs + robot_nav_msgs + robot_nav_2d_msgs + robot_nav_2d_utils + robot_sensor_msgs + robot_map_msgs + robot_protocol_msgs + robot_xmlrpcpp + + DEPENDS + Boost + ) + + include_directories(include) + + # Dependency vào dạng SYSTEM: `robot/robot.h` kéo theo `console.h`, file này khai vài chục hằng màu + # ở namespace scope và sinh đúng số đó warning -Wunused-variable. Để chúng lẫn vào output sẽ chôn + # mất warning THẬT của gói — mà cờ -Wall -Wextra ở dưới có mặt chính là để thấy những warning đó. + include_directories(SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + +endif() + + +# ======================================================== +# Core library — logic thuần, không I/O. +# +# Tách riêng khỏi plugin để test lõi không phải kéo theo contract host, và để bất kỳ ai muốn nhúng +# state machine vào runtime khác cũng dùng lại được. +# ======================================================== +add_library(move_base2_core SHARED + src/navigation_state.cpp + src/state_machine.cpp + src/velocity_arbiter.cpp + src/control_loop.cpp + src/config/move_base2_config.cpp + src/runners/recovery_runner.cpp + src/runners/action_runner.cpp + src/runners/planner_runner.cpp + src/runners/controller_runner.cpp + src/io/sensor_gateway.cpp + src/bridges/mission_adapter_bridge.cpp +) + +# Gói này KHÔNG kế thừa cờ -w của pnkx_nav_core: warning ở đây phải nhìn thấy được. +target_compile_options(move_base2_core PRIVATE -Wall -Wextra) + +target_include_directories(move_base2_core + PUBLIC + $ + $ +) + + +# ======================================================== +# 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. +# ======================================================== +add_library(move_base2 SHARED + src/navigation_server.cpp + src/move_base2_plugin.cpp +) + +target_compile_options(move_base2 PRIVATE -Wall -Wextra) + +target_include_directories(move_base2 + PUBLIC + $ + $ +) + +set_target_properties(move_base2 PROPERTIES POSITION_INDEPENDENT_CODE ON) + + +# ======================================================== +# Linking +# ======================================================== +if(BUILDING_WITH_CATKIN) + + add_dependencies(move_base2_core + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + + add_dependencies(move_base2 + move_base2_core + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + + target_link_libraries(move_base2_core + PUBLIC + ${catkin_LIBRARIES} + + PRIVATE + Boost::boost + Threads::Threads + ${TF3_LIBRARY} + ) + + target_link_libraries(move_base2 + PUBLIC + move_base2_core + ${catkin_LIBRARIES} + + PRIVATE + Boost::boost + Boost::system + Boost::filesystem + Threads::Threads + ${CMAKE_DL_LIBS} + ${TF3_LIBRARY} + ) + +else() + + target_include_directories(move_base2_core + PRIVATE + ${STANDALONE_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + + target_include_directories(move_base2 + PRIVATE + ${STANDALONE_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + + target_link_libraries(move_base2_core + PUBLIC + ${PACKAGES_DIR} + + PRIVATE + Boost::boost + Threads::Threads + ${TF3_LIBRARY} + ) + + target_link_libraries(move_base2 + PUBLIC + move_base2_core + ${PACKAGES_DIR} + + PRIVATE + Boost::boost + Boost::system + Boost::filesystem + Threads::Threads + ${CMAKE_DL_LIBS} + ${TF3_LIBRARY} + ) + + set_target_properties(move_base2_core move_base2 PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + ) + +endif() + + +# ======================================================== +# Install +# ======================================================== +if(BUILDING_WITH_CATKIN) + + install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + +else() + + install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + +endif() + + +# ======================================================== +# Tests +# ======================================================== +option(BUILD_MOVE_BASE2_TESTS "Build move_base2 tests" ON) + +if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN) + + if(NOT COMMAND catkin_add_gtest) + message(FATAL_ERROR "catkin_add_gtest NOT FOUND") + endif() + + # Plugin global planner chỉ dùng cho test — nạp qua Boost.DLL đúng đường runtime đi, không link + # thẳng. Đặt trong nhánh test để không bao giờ lọt vào bản cài đặt. + foreach(test_plugin move_base2_test_global_planner move_base2_test_local_planner) + if(test_plugin STREQUAL "move_base2_test_global_planner") + set(test_plugin_src test/plugins/test_global_planner.cpp) + else() + set(test_plugin_src test/plugins/test_local_planner.cpp) + endif() + + add_library(${test_plugin} SHARED ${test_plugin_src}) + + target_compile_options(${test_plugin} PRIVATE -Wall -Wextra) + + target_include_directories(${test_plugin} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + set_target_properties(${test_plugin} PROPERTIES POSITION_INDEPENDENT_CODE ON) + + target_link_libraries(${test_plugin} + PRIVATE + ${catkin_LIBRARIES} + Boost::boost + ${CMAKE_DL_LIBS} + ) + endforeach() + + set(MOVE_BASE2_TESTS + state_machine_test + velocity_arbiter_test + walking_skeleton_test + config_validation_test + action_runner_test + recovery_runner_test + sensor_gateway_test + navigation_server_test + planner_runner_test + controller_runner_test + mission_adapter_bridge_test + ) + + foreach(test_name ${MOVE_BASE2_TESTS}) + catkin_add_gtest(${test_name} test/${test_name}.cpp) + + if(TARGET ${test_name}) + # catkin_add_gtest đặt target EXCLUDE_FROM_ALL; mở ra để binary có mặt sau `catkin_make` + # thường, đúng quy trình verify của repo (chạy thẳng ./devel/lib//). + set_target_properties(${test_name} PROPERTIES EXCLUDE_FROM_ALL FALSE) + + target_compile_options(${test_name} PRIVATE -Wall -Wextra) + + target_include_directories(${test_name} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/test + SYSTEM PRIVATE + ${catkin_INCLUDE_DIRS} + ) + + # catkin_add_gtest đã dùng target_link_libraries dạng plain, nên phần bổ sung cũng phải plain. + target_link_libraries(${test_name} + move_base2_core + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + yaml-cpp + ${CMAKE_DL_LIBS} + ) + + # Plugin recovery được nạp qua Boost.DLL từ devel/lib, không link thẳng: đúng đường runtime đi. + # ctest không mang theo PNKX_NAV_CORE_* của shell nên binary tự trỏ. + target_compile_definitions(${test_name} PRIVATE + MOVE_BASE2_TEST_CONFIG_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/config" + MOVE_BASE2_TEST_LIBRARY_DIR="${CATKIN_DEVEL_PREFIX}/lib" + ) + endif() + endforeach() + + # `NavigationServer` nằm trong thư viện plugin `move_base2`, không phải trong `move_base2_core`. + if(TARGET navigation_server_test) + target_link_libraries(navigation_server_test move_base2) + endif() + + # Plugin phải có mặt trong devel/lib trước khi test chạy — nó được nạp qua Boost.DLL lúc chạy. + if(TARGET planner_runner_test) + add_dependencies(planner_runner_test move_base2_test_global_planner) + endif() + + if(TARGET controller_runner_test) + add_dependencies(controller_runner_test move_base2_test_local_planner) + endif() + +endif() diff --git a/README.md b/README.md new file mode 100644 index 0000000..5ae8137 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ +# move_base2 + +Navigation runtime thế hệ 2: nhận yêu cầu di chuyển, điều phối global planner, local planner và +recovery behavior qua một state machine tường minh, và phát lệnh vận tốc từ **đúng một nguồn** tại +mỗi thời điểm. + +Gói hiện thực contract host `robot::move_base_core::BaseNavigation` và được nạp bằng Boost.DLL +(alias `MoveBase2`) như mọi plugin khác của workspace. + +## Điểm khác biệt + +Phần lõi quyết định — state machine và bộ trọng tài vận tốc — là **logic thuần, không I/O**. Nó không +đụng costmap, không đụng TF, không log, không cấp phát trong vòng lặp. Vì vậy hành vi của nó kiểm +được bằng bảng chuyển trạng thái thay vì phải chạy robot, và mọi thay đổi hành vi đều để lại dấu vết +trong test. + +Mọi phụ thuộc ra ngoài đi qua **port**: thời gian, pose, global planner, local planner, recovery, +mission. Lõi không biết mission framework hay recovery framework nào đang chạy phía sau. + +## Cấu trúc + +``` +include/move_base2/ +├── ports/ # sáu cổng ra: clock, pose, planner, controller, recovery, mission +├── core/ # navigation_request, navigation_state, state_machine, velocity_arbiter +├── control_loop.h # một control cycle +└── navigation_server.h # facade hiện thực BaseNavigation +src/ +test/ # fake_ports.h + ba bộ test +docs/ # ARCHITECTURE.md, STATE_MACHINE.md, THREADING.md +``` + +`docs/STATE_MACHINE.md` là **nguồn chuẩn** cho hành vi chuyển state. Đổi hành vi thì sửa tài liệu đó +trước, sửa test, rồi mới sửa code. + +## Luồng runtime + +``` +host / mission layer + │ moveTo / dockTo / moveStraightTo / rotateTo + ▼ +NavigationServer::submit() quy về một NavigationRequest + │ từ chối ngay tại cửa nếu goal hỏng hoặc không nạp được planner + ▼ +ControlLoop::step() mỗi control cycle: + │ 1. đọc thời gian và pose, tính dt thật + │ 2. chạy state machine với phản hồi của cycle TRƯỚC + │ 3. thi hành output: nhận yêu cầu, đẩy plan, start/tick recovery, chạy controller + │ 4. đưa lệnh ứng viên qua bộ trọng tài + │ 5. báo kết quả cho mission layer nếu state machine yêu cầu + ▼ + cmd_vel +``` + +Nguồn vận tốc chỉ có hai khả năng khác `kNone`: local planner khi đang ở `CONTROLLING`, recovery +behavior khi đang ở `RECOVERING`. Đổi nguồn luôn chèn đúng một cycle vận tốc 0. + +## Build và test + +```bash +cd /home/duongtd/T800_ws +catkin_make --pkg nav_test_harness +catkin_make --pkg move_base2 + +./devel/lib/move_base2/state_machine_test +./devel/lib/move_base2/velocity_arbiter_test +./devel/lib/move_base2/walking_skeleton_test +``` + +Gói build với `-Wall -Wextra` qua `target_compile_options` và **không** kế thừa cờ tắt warning của +cây nav core. + +## Cấu hình + +Ở trạng thái hiện tại, cấu hình được truyền vào bằng struct `ControlLoopConfig` (có `validate()` và +`describe()`). Phần đọc từ YAML qua `robot::NodeHandle` thuộc bước nối dây runtime; khi thêm, file +cấu hình đang có hiệu lực sẽ nằm trong cây config của nav core, không nằm trong gói này — bản trong +`test/` nếu có chỉ phục vụ test và phải chạy kèm biến môi trường trỏ vào đúng thư mục đó. + +## 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 +`docs/ARCHITECTURE.md`. diff --git a/docs/ARCHITECTURE.md b/docs/ARCHITECTURE.md new file mode 100644 index 0000000..5e0e66f --- /dev/null +++ b/docs/ARCHITECTURE.md @@ -0,0 +1,122 @@ +# Kiến trúc + +## Ba lớp + +``` +┌──────────────────────────────────────────────────────────────────────┐ +│ Biên (adapter) │ +│ │ +│ NavigationServer ──implements──▶ move_base_core::BaseNavigation │ +│ · quy 6 entry point di chuyển về 1 NavigationRequest │ +│ · nhận dữ liệu sensor từ host │ +│ · kết xuất trạng thái ra kiểu mà host mong đợi │ +└──────────────────────────────────┬───────────────────────────────────┘ + │ +┌──────────────────────────────────▼───────────────────────────────────┐ +│ Điều phối │ +│ │ +│ ControlLoop — một control cycle: gom dữ liệu, chạy state machine, │ +│ thi hành output, phát lệnh vận tốc │ +└──────────────────────────────────┬───────────────────────────────────┘ + │ +┌──────────────────────────────────▼───────────────────────────────────┐ +│ Lõi quyết định (logic thuần, không I/O) │ +│ │ +│ StateMachine — bảng chuyển state │ +│ VelocityArbiter — ai được phát lệnh, và lệnh đó có an toàn không │ +│ NavigationRequest — kiểu dữ liệu duy nhất đi vào lõi │ +└──────────────────────────────────────────────────────────────────────┘ + │ + ▼ (qua port, không qua kiểu cụ thể) + ClockPort PosePort PlannerPort ControllerPort + RecoveryPort MissionPort ActionPort +``` + +Lõi không đụng costmap, không đụng TF, không log, không cấp phát trong vòng lặp. Vì vậy nó kiểm được +bằng bảng thay vì phải dựng runtime hay chạy robot. + +## Chiều phụ thuộc + +``` +move_base2 ──▶ mission framework + ──▶ recovery framework +``` + +**Một chiều, không có đường ngược.** Hai framework kia không biết `move_base2` tồn tại và không biết +lẫn nhau. Hệ quả có thật, không phải hình thức: + +- Ba gói test được độc lập. +- Không có vòng phụ thuộc trong build. +- `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. + +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 +``` + +## Vì sao là port chứ không phải gọi thẳng + +Bảy port đều nhỏ và đều tồn tại vì một lý do vận hành cụ thể: + +| Port | Lý do tồn tại | +|---|---| +| `ClockPort` | Không có nguồn thời gian tiêm được thì không cách nào kiểm hành vi khi control loop chạy chậm hơn chu kỳ cấu hình — đúng lớp lỗi mà dead-reckoning theo chu kỳ danh nghĩa mắc phải | +| `PosePort` | Contract "trả false = dừng an toàn" phải kiểm được mà không cần dựng TF thật | +| `PlannerPort` | Gộp hai overload `makePlan` của interface gốc thành một; "có Order hay không" chỉ là một nhánh nhỏ bên trong | +| `ControllerPort` | Giữ nguyên bộ hàm và **thứ tự gọi** của interface được bọc (hỏi đã tới đích trước, chỉ khi chưa mới tính lệnh) | +| `RecoveryPort` | Chuẩn hoá kết quả tick về ngôn ngữ của `move_base2`, để lõi không phải include recovery framework | +| `MissionPort` | Luồng một chiều có callback, thay cho việc phía mission phải poll trạng thái navigation | +| `ActionPort` | (D8) Runtime điều phối trọn một mission — nav xong chạy nốt action rồi mới báo kết quả. Tick-based cùng nhịp control loop như recovery, và tick của nó **không có** vận tốc: action cần chuyển động phải là motion profile, không phải action | + +## 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. + +**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 +nhau là tính chất cấu trúc, không phải thứ phải nhớ khoá bằng tay. + +**Từ chối tại cửa.** Goal có quaternion hỏng, toạ độ không hữu hạn, hoặc profile không nạp được +planner đều bị từ chối ngay trong `submit()` kèm lý do — 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. + +**Phản hồi trễ một cycle.** State machine chạy với phản hồi thu được từ cycle trước, rồi mới gọi +cổng. Điều này là tường minh và có chủ đích: nó cắt vòng "gọi để biết nên gọi gì", nhờ vậy output của +state machine là một danh sách hành động thuần tuý. + +**Lệnh 0 là tức thì.** Nguồn `kNone` phát đúng 0, không giảm tốc dần. Lệnh vận tốc bị chốt lại ở tầng +dưới, nên nếu control loop dừng giữa lúc đang giảm tốc thì lệnh khác 0 cuối cùng vẫn còn hiệu lực. +Việc giảm tốc theo động học thuộc về bộ điều khiển bánh xe, nơi biết tải và ma sát thật. + +## Trạng thái hiện tại và phần còn thiếu + +Đã có và chạy được: + +- Toàn bộ lõi quyết định, có test phủ đủ bảng chuyển state. +- `ControlLoop` chạy end-to-end với thành phần giả. +- `NavigationServer` hiện thực đủ contract host. +- `SensorGateway`: dữ liệu cảm biến đi từ contract host tới đúng layer của hai costmap, có phát lại + static map nhận trước khi costmap tồn tại. Đây là file duy nhất trong gói include `robot_costmap_2d`. +- `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: + +- 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). diff --git a/docs/STATE_MACHINE.md b/docs/STATE_MACHINE.md new file mode 100644 index 0000000..54505f8 --- /dev/null +++ b/docs/STATE_MACHINE.md @@ -0,0 +1,119 @@ +# State machine + +Đây là **nguồn chuẩn** cho hành vi chuyển trạng thái của navigation runtime. Code trong +`src/state_machine.cpp` phải khớp với bảng dưới đây, và `test/state_machine_test.cpp` là thứ chứng +minh điều đó — mỗi transition trong tài liệu này có ít nhất một test tương ứng. + +Khi cần đổi hành vi: sửa tài liệu này trước, sửa test, rồi mới sửa code. + +## Bảng state + +| 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)` | +| `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` | +| `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` | +| `SUCCEEDED` / `ABORTED` / `CANCELLED` | không ai (0) | như trên | terminal; báo kết quả rồi về `IDLE` ở cycle kế tiếp | + +## Thứ tự ưu tiên trong một cycle + +Ở mọi state đang chạy, sự kiện được xét theo đúng thứ tự sau. Thứ tự này là một phần của contract: + +1. `cancel_requested` +2. `pause_requested` +3. plan mới sẵn sàng +4. phản hồi của controller / recovery / action +5. các ngưỡng kiên nhẫn và chống quẩn + +## Ba khác biệt so với runtime thế hệ 1 + +1. **`RECOVERING` thay cho `CLEARING`, và là state có thời lượng.** Nhiều control cycle, không phải + một lời gọi blocking. Bắt buộc như vậy vì recovery thế hệ 2 trả kết quả từng tick và có thể phát + vận tốc — nghĩa là quyền phát cmd_vel phải chuyển tay từ local planner sang recovery và ngược lại. + +2. **`PAUSED` và `CANCELLING` là state thật**, không phải cờ đọc rải rác trong vòng lặp. Đường huỷ ở + bản cũ được viết hai lần, ở nhánh `try` và nhánh `catch`, gần 80 dòng giống hệt nhau. + +3. **`SUCCEEDED`/`ABORTED`/`CANCELLED` là state**, không phải `return` giữa hàm. Đây là cơ chế giữ + bất biến báo-kết-quả-một-lần: cờ `report_outcome` chỉ bật tại cycle bước vào state terminal, và + state terminal chỉ sống đúng một cycle. + +4. **`EXECUTING_ACTIONS` (D8): runtime điều phối trọn một mission.** Tới goal chưa phải là xong — + còn action của mission (nâng/hạ, sạc, chờ…) phải chạy nốt, và kết quả chỉ được báo **sau action + cuối**, để mission layer thấy trọn một chặng nav + action. Yêu cầu `has_goal == false` bỏ qua + `PLANNING`/`CONTROLLING` và vào thẳng state này. + +## Action của mission (D8) — các quyết định robotics + +- **Không vận tốc trong khi chạy action.** `EXECUTING_ACTIONS` thuộc nhóm `mustBeStopped`; action + cần chuyển động phải được mô hình hoá thành motion profile của navigation. Đây là hàng rào chống + hai nguồn điều khiển. +- **`pause()` đóng băng, không huỷ.** Khác `RECOVERING` (bị huỷ khi tạm dừng vì dead-reckon theo + thời gian), action thiết bị không idempotent — chạy lại một lần nâng kệ từ đầu không chắc an + toàn. Tạm dừng chỉ ngừng tick; `resume()` tick tiếp đúng action dở dang, không `start` lại. +- **Action hỏng → `ABORTED` thẳng, không qua 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. Mission layer là + nơi quyết định làm gì với phần còn lại của order. +- **Mất pose không chặn action.** Robot đứng yên, thao tác thiết bị không cần định vị; vận tốc vẫn + bị ép về 0 như mọi state phải dừng. Đồng hồ kiên nhẫn của planner/controller không chạy trong + state này — action dài (sạc pin) không được phép bị tính là "controller hỏng". +- **Timeout 3 tầng, tầng 1 là chính.** (1) Mỗi ActionHandler tự timeout theo hiểu biết thiết bị + của nó và trả `failed` — chỉ handler biết "nâng kệ quá 20 s là bất thường" còn "sạc 30 phút là + bình thường"; (2) `action_patience` là lưới cuối ở tầng navigation cho handler treo, **mặc định + tắt**, tính cho từng action, quá hạn thì cancel action rồi `ABORTED`; (3) `mission_timeout` của + mission layer đo cả chặng. Trạng thái host trong lúc chạy action là `ACTIVE` (không phải + `CONTROLLING` — host VDA5050 suy `driving = true` từ `CONTROLLING`, mà robot đang đứng yên). + +## Đồng hồ và bộ đếm + +| Biến | Đặt lại khi | Không đặt lại khi | +|---|---|---| +| `last_valid_plan_` (đo `planner_patience`) | nhận yêu cầu mới; bắt đầu một chu kỳ lập plan mới; tiếp tục sau tạm dừng | — | +| `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 | — | +| `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 +mới hai đồng hồ đó mỗi lần có plan, vòng lặp `CONTROLLING → PLANNING → CONTROLLING` sẽ liên tục gia +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. + +## 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ể: + +- Nguồn vận tốc bị ép về `kNone` ở **mọi** state, kể cả `RECOVERING`. +- Controller không được gọi. +- Recovery **vẫn** được tick, để behavior tự báo lỗi theo contract của nó; lệnh nó sinh ra bị chặn. +- Ở `CONTROLLING`, mất pose được tính là "không sinh được lệnh" nhưng **không** chuyển sang + `PLANNING`: lập lại plan không giúp gì khi vấn đề là định vị, và nhảy sang `PLANNING` sẽ khiến lý + do vào recovery bị ghi nhận sai thành "lập plan hỏng". +- Các đồng hồ kiên nhẫn vẫn chạy, nên mất TF kéo dài cuối cùng vẫn dẫn tới `RECOVERING` rồi + `ABORTED` — không treo im lặng. + +## Bất biến + +Bốn tính chất sau được chốt lại ở cuối mỗi lần chuyển state và được test kiểm ở từng cycle: + +1. Mỗi cycle có **đúng một** nguồn vận tốc. +2. `run_controller`, `tick_recovery` và `tick_action` đôi một không bao giờ cùng bật. +3. Ở state phải dừng (mọi state trừ `CONTROLLING` và `RECOVERING` — gồm cả `EXECUTING_ACTIONS`), + nguồn vận tốc luôn là `kNone`. +4. `report_outcome` bật đúng một lần cho mỗi yêu cầu — kể cả yêu cầu có action: chỉ báo sau action + cuối (D8). + +## Vì sao `CANCELLING` luôn kết thúc + +`CANCELLING` chờ `robot_stopped`. Vì nguồn vận tốc ở state này đã là `kNone` và bộ trọng tài phát +lệnh 0 tức thì, `robot_stopped` thành true ngay ở cycle kế tiếp. Do đó state này kết thúc sau hữu hạn +cycle mà không cần thêm tham số timeout nào. + +Ở bước nối dây sau, `robot_stopped` nên được lấy từ vận tốc **đo được** (odometry) thay vì từ lệnh +đã phát. Đó là lý do nó là một trường dữ liệu vào của state machine chứ không phải thứ state machine +tự tính. diff --git a/docs/THREADING.md b/docs/THREADING.md new file mode 100644 index 0000000..c07edce --- /dev/null +++ b/docs/THREADING.md @@ -0,0 +1,72 @@ +# Mô hình thread và quyền sở hữu + +## Bảng thread + +| Thread | Sở hữu | Chu kỳ | Ghi chú | +|---|---|---|---| +| `control` | state machine, bộ trọng tài vận tốc, controller, recovery, **cmd_vel** | `controller_frequency` | **Thread duy nhất được phát cmd_vel.** Toàn bộ `ControlLoop::step()` chạy ở đây | +| `planner` | global planner, bộ đệm plan | `planner_frequency` hoặc theo biến điều kiện | *Chưa có ở trạng thái hiện tại* — xem mục dưới | +| `mission_event` | hàng đợi sự kiện mission | theo sự kiện | Thuộc mission framework, không thuộc gói này | +| `mission_exec` | điều phối chặng mission | theo sự kiện | Thuộc mission framework. Chỉ gọi callback, **không được block** | +| host (ROS / C#) | nhận dữ liệu sensor, gọi API `BaseNavigation` | tuỳ host | Ghi vào vùng dữ liệu sensor qua mutex | + +## Bất biến quan trọng nhất + +> Chỉ thread `control` được phát cmd_vel. + +Đây là lý do recovery được **tick từ control thread** thay vì chạy trong thread riêng. Recovery thế +hệ 2 có thể phát vận tốc; nếu nó chạy ở thread riêng thì sẽ có hai bộ điều khiển cùng ghi vào một +đường lệnh, và không có cách nào xác định được lệnh nào tới trước. Đó là hai bộ điều khiển tranh +nhau, không phải hai tác vụ song song. + +Bất biến này được củng cố ở hai chỗ, độc lập nhau: + +1. **Cấu trúc:** state machine cho ra đúng một `velocity_source` mỗi cycle, và `run_controller` với + `tick_recovery` không bao giờ cùng bật. +2. **Cổng ra:** mọi lệnh đều đi qua `VelocityArbiter`, và bộ trọng tài chỉ được gọi một lần trong + `ControlLoop::step()`. + +## Mutex + +| Mutex | Bảo vệ | Ai giữ | +|---|---|---| +| `NavigationServer::data_mutex_` | bản đồ tĩnh, laser scan, point cloud, depth camera, odometry, footprint | host khi ghi, control thread khi đọc | +| mutex của costmap | dữ liệu costmap | phần nối dây; **lấy lại mỗi lần dùng, không cache** | + +`ControlLoop` và `StateMachine` **không** có mutex nào và cố ý như vậy: chúng chỉ chạy trên control +thread. Thêm mutex vào đó sẽ che mất việc có ai đó gọi sai thread. + +## Con trỏ costmap + +Con trỏ lấy từ `getCostmap()` là **non-owning và có thể bị thay giữa hai cycle**. Cache lại chính là +nguyên nhân lỗi double-free đã ghi nhận trong workspace. Quy tắc áp cho mọi hiện thực port: + +- Lấy lại con trỏ ở đầu mỗi lần dùng. +- Không giữ tham chiếu qua nhiều cycle. +- Không giữ tham chiếu qua ranh giới thread. + +## Trạng thái hiện tại: một thread + +`ControlLoop` hiện chạy đồng bộ, một thread, và lập plan ngay trong `step()`. Đây là lựa chọn có chủ +đích cho bước dựng khung: phần quyết định kiểm được đầy đủ mà không cần thread nào, nên test chạy tất +định và không có race. + +Khi thêm thread planner ở bước sau: + +- Giữ nguyên mô hình bộ đệm plan ba lớp của bản cũ. Nó đang hoạt động tốt và là đoạn code tinh tế — + bê nguyên si trước, không "cải tiến" cùng lúc với việc chuyển sang kiến trúc mới. +- Điểm nối là `ControlLoop::runPlanner()`: thay lời gọi đồng bộ bằng việc đánh thức thread planner và + đọc kết quả từ bộ đệm. +- `PlannerFeedback` không đổi. Đó chính là mục đích của việc tách nó thành một enum: state machine + không cần biết plan được tính đồng bộ hay bất đồng bộ. + +## Vòng đời + +| Đối tượng | Ai sở hữu | +|---|---| +| `NavigationServer` | host, qua `shared_ptr` do factory plugin trả về | +| `ControlLoop`, `StateMachine`, `VelocityArbiter` | `NavigationServer`, theo giá trị | +| Mọi cổng (`ControlLoopDeps`) | **không sở hữu**; phải sống lâu hơn `ControlLoop` | + +`ControlLoopDeps` chỉ chứa con trỏ trần và điều này là cố ý: nó nói rõ rằng control loop không sở hữu +gì cả. Bên nối dây chịu trách nhiệm giữ các cổng sống đủ lâu. diff --git a/include/move_base2/bridges/mission_adapter_bridge.h b/include/move_base2/bridges/mission_adapter_bridge.h new file mode 100644 index 0000000..e968044 --- /dev/null +++ b/include/move_base2/bridges/mission_adapter_bridge.h @@ -0,0 +1,142 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — nối MissionPort với framework mission_adapters. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_ +#define MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace mission_adapters +{ +class MissionManager; +} + +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 + * @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: + * - @ref MissionPort — phía `move_base2` nhìn vào; + * - `mission_adapters::NavigationClient` — phía mission layer nhìn vào. + * + * ## Biên thread nằm ở đây, có chủ đích + * + * `MissionExecutor` gọi `dispatch()` từ **thread của nó**, còn `ControlLoop` không thread-safe và + * chỉ được chạm từ control thread. Nếu `dispatch()` gọi thẳng xuống navigation thì hai thread cùng + * ghi `pending_request_` của control loop. + * + * Vì vậy `dispatch()` chỉ **cất mission lại** rồi trả về ngay (executor cũng không được phép block — + * xem bảng threading của plan). Control thread gọi @ref pumpPendingRequest mỗi cycle; chính ở đó + * mission mới được chuyển thành @ref NavigationRequest và đẩy qua callback. + * + * ## Vì sao chỉ giữ MỘT mission chờ + * + * `MissionManager::nextMission()` chỉ trả mỗi mission đúng một lần và chỉ giao chặng mới sau khi + * chặng cũ kết thúc, nên trong thực tế không bao giờ có hai mission cùng chờ. Nếu vẫn xảy ra thì + * mission mới đè mission cũ và việc đó được **đếm lại** (@ref droppedRequests) — im lặng ở đây + * nghĩa là một chặng biến mất mà fleet master vẫn chờ nó. + */ +class MissionAdapterBridge : public MissionPort, public mission_adapters::NavigationClient +{ +public: + /// @brief Được gọi khi navigation cần dừng chặng đang chạy. Chạy trên **control thread**. + using CancelCallback = std::function; + + MissionAdapterBridge(); + ~MissionAdapterBridge() override; + + MissionAdapterBridge(const MissionAdapterBridge&) = delete; + MissionAdapterBridge& operator=(const MissionAdapterBridge&) = delete; + + /** + * @brief Gắn mission manager để @ref reportOutcome có chỗ báo về. + * @param manager **Non-owning**, được phép null (chạy không có mission layer). + */ + void attach(mission_adapters::MissionManager* manager); + + /** + * @brief Đăng ký cách dừng navigation. + * + * Không nằm trong @ref MissionPort vì đó là cổng một chiều theo thiết kế: mission đẩy chặng + * xuống, navigation báo kết quả lên. Nhưng `NavigationClient::cancelActive` bắt buộc phải có + * đường tác động ngược, nên nó đi qua callback riêng này. + */ + void setCancelCallback(CancelCallback callback); + + /** + * @brief Chuyển mission đang chờ (nếu có) thành yêu cầu và đẩy qua callback đã đăng ký. + * + * **Chỉ gọi từ control thread**, mỗi cycle một lần. Đây là chỗ duy nhất biên thread được vượt qua. + * + * @return true nếu có một yêu cầu vừa được đẩy xuống. + */ + bool pumpPendingRequest(); + + // ================================================================================================ + // MissionPort — phía move_base2 + // ================================================================================================ + + void setRequestCallback(RequestCallback callback) override; + void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) override; + bool hasActiveMission() const override; + void start() override; + void stop() override; + + // ================================================================================================ + // mission_adapters::NavigationClient — phía mission layer + // ================================================================================================ + + bool dispatch(const std::shared_ptr& mission) override; + void cancelActive(mission_adapters::MissionId id) override; + + // ================================================================================================ + // Chẩn đoán + // ================================================================================================ + + /// @brief Số mission bị đè vì tới khi mission trước chưa kịp được đẩy xuống. + std::size_t droppedRequests() const; + + /// @brief Số lần báo kết quả mà mission layer không nhận (outcome tới trễ, mission đã bị thay). + std::size_t staleOutcomes() const; + + /// @brief Chuyển một mission thành yêu cầu navigation. Phơi ra để test được phần chuyển đổi. + static NavigationRequest toRequest(const mission_adapters::Mission& mission); + +private: + mutable std::mutex mutex_; + + mission_adapters::MissionManager* manager_ = nullptr; ///< Non-owning, có thể null. + RequestCallback request_callback_; + CancelCallback cancel_callback_; + + std::shared_ptr pending_; + bool running_ = false; + + std::size_t dropped_requests_ = 0; + std::size_t stale_outcomes_ = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_ diff --git a/include/move_base2/config/move_base2_config.h b/include/move_base2/config/move_base2_config.h new file mode 100644 index 0000000..01e3e38 --- /dev/null +++ b/include/move_base2/config/move_base2_config.h @@ -0,0 +1,153 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — toàn bộ tham số runtime, đọc từ YAML và validate ở một chỗ. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_ +#define MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_ + +#include + +#include + +#include +#include + +namespace move_base2 +{ + +/** + * @struct MoveBase2Config + * @brief Cấu hình đầy đủ của navigation runtime. + * + * 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; + * 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. + * + * Điểm (3) là khác biệt có chủ đích so với bản cũ: nó đọc param không kiểm miền, và có chỗ đọc + * `max_planning_retries` vào `double` rồi gán sang `int32_t` — một giá trị âm hoặc quá lớn đi thẳng + * vào vòng điều khiển mà không ai biết. + */ +struct MoveBase2Config +{ + // --- Nhịp chạy ------------------------------------------------------------------------------- + + /// [Hz] Nhịp control loop. Đây cũng là nhịp tick recovery và action. + double controller_frequency = 20.0; + + /// [Hz] Nhịp lập plan lại khi đang bám plan. 0 = chỉ lập khi cần (theo yêu cầu của state machine). + double planner_frequency = 0.0; + + /// [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; + + // --- Hành vi chuyển state -------------------------------------------------------------------- + + StateMachineConfig state_machine; + + // --- Hàng rào vận tốc cuối cùng --------------------------------------------------------------- + + VelocityLimits velocity; + + // --- Đường vào cảm biến ----------------------------------------------------------------------- + + /// Đọc từ namespace con `sensors`. Xem @ref SensorGatewayConfig::laser_sor_enabled về lý do + /// bộ lọc laser mặc định tắt. + SensorGatewayConfig sensors; + + // --- Ánh xạ profile -> planner ---------------------------------------------------------------- + + ProfileBinding position; + ProfileBinding docking; + ProfileBinding go_straight; + ProfileBinding rotate; + + // --- Namespace cho các thành phần nạp plugin --------------------------------------------------- + + /// Namespace chứa danh sách recovery behavior (`/behaviors`) trong YAML. + std::string recovery_namespace = "recovery"; + + /// Namespace chứa danh sách action handler (`/handlers`) trong YAML. + std::string action_namespace = "actions"; + + /// Namespace chứa cấu hình mission layer. + std::string mission_namespace = "mission_adapters"; + + // --- Frame ------------------------------------------------------------------------------------ + + /// Frame mà goal được quy về trước khi lập plan. + std::string global_frame = "map"; + + /// Frame gắn với thân robot. + std::string robot_base_frame = "base_link"; + + /** + * @brief Đọc toàn bộ tham số từ @p nh. + * + * Khoá thiếu thì giữ default và **log rõ khoá nào** — im lặng dùng default là cách một tham số + * quan trọng biến mất mà không ai phát hiện. Hàm này không validate; gọi @ref validate sau. + * + * @param nh NodeHandle đã scope vào namespace của runtime. + */ + void fromNodeHandle(robot::NodeHandle& nh); + + /** + * @brief Đọc theo schema move_base gen-1 (`move_base_common_params.yaml`, khoá ở root). + * + * Cho phép chuyển sang move_base2 mà KHÔNG phải viết lại cây config đang chạy. Ánh xạ: + * - `position/docking/go_straight/rotate_planner_name` -> `.local_planner_name`; + * - `: base_global_planner` -> `.global_planner_name`, thiếu thì rơi về + * `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. + * + * 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 + * là "tắt đồng hồ". Giá trị <= 0 được dịch thành đúng MỘT chu kỳ điều khiển — gen-1 cũng chỉ + * phản ứng được ở độ phân giải cycle nên hành vi giữ nguyên. + * 2. default của gen-1 khác gen-2: `robot_base_frame` = "base_footprint" (gen-2: "base_link"), + * tolerance = 0.2 (gen-2: 0.15/0.10). Chế độ legacy giữ default gen-1. + * + * @param nh NodeHandle nhìn thấy các khoá gen-1 (thường là root "~"). + */ + 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. + * + * 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. + */ + static MoveBase2Config load(robot::NodeHandle& root_nh); + + /** + * @brief Kiểm miền giá trị của mọi tham số, gồm cả các struct con. + * @param[out] error Mô tả tham số sai đầu tiên gặp phải; chỉ ghi khi hàm trả false. + * + * @warning **Ràng buộc thứ tự khởi tạo:** `state_machine.recovery_behavior_count` không đến từ + * YAML mà là số behavior `RecoveryRunner` nạp được **thật**. Trình tự đúng là: + * `fromNodeHandle()` → `RecoveryRunner::configure()` → gán + * `state_machine.recovery_behavior_count = runner.behaviorCount()` → `validate()`. + * Đọc con số này từ YAML thì một behavior hỏng vẫn khiến state machine tin là còn đường + * phục hồi, và lỗi đầu tiên sẽ dẫn thẳng tới ABORTED mà không ai hiểu vì sao. + */ + bool validate(std::string& error) const; + + /// @brief Kết xuất thành text nhiều dòng, để log đúng một lần lúc khởi tạo. + std::string describe() const; + + /// @brief Phần cấu hình mà @ref ControlLoop cần, trích ra từ bản đầy đủ này. + ControlLoopConfig toControlLoopConfig() const; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_ diff --git a/include/move_base2/control_loop.h b/include/move_base2/control_loop.h new file mode 100644 index 0000000..b9b481b --- /dev/null +++ b/include/move_base2/control_loop.h @@ -0,0 +1,292 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — một control cycle: gom dữ liệu, chạy state machine, thi hành kết quả. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CONTROL_LOOP_H_ +#define MOVE_BASE2_CONTROL_LOOP_H_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace move_base2 +{ + +/** + * @struct ControlLoopDeps + * @brief Các cổng mà control loop cần. Tất cả đều **non-owning** và bắt buộc khác null. + * + * @ref mission và @ref action được phép null: goal có thể đến thẳng từ contract host mà không qua + * mission layer nào, và một hệ không có thiết bị thì không cần action. Yêu cầu MANG action sẽ bị + * từ chối tại @ref ControlLoop::submit khi @ref action null — từ chối sớm thay vì kẹt giữa chừng. + */ +struct ControlLoopDeps +{ + ClockPort* clock = nullptr; + PosePort* pose = nullptr; + PlannerPort* planner = nullptr; + ControllerPort* controller = nullptr; + RecoveryPort* recovery = nullptr; + MissionPort* mission = nullptr; ///< Có thể null. + 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. + * + * 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. + */ +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] +}; + +/** + * @struct ControlLoopConfig + * @brief Tham số của control loop. + */ +struct ControlLoopConfig +{ + StateMachineConfig state_machine; + VelocityLimits velocity; + + /// [s] Chu kỳ danh nghĩa, chỉ dùng cho dt của cycle ĐẦU TIÊN khi chưa có mốc thời gian trước đó. + double nominal_control_period = 0.05; + + /// Frame gắn với thân robot. Lệnh vận tốc phát ra được đóng dấu bằng frame này — cmd_vel là vận + /// tốc trong hệ thân xe, không phải hệ bản đồ hay odom. + std::string robot_base_frame = "base_link"; + + /// Á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; + + bool validate(std::string& error) const; + std::string describe() const; +}; + +/** + * @class ControlLoop + * @brief Thân vòng điều khiển, chạy một cycle mỗi lần gọi @ref step. + * + * Trình tự trong một cycle, cố định và không được đảo: + * 1. Đọc thời gian, đọc pose, tính dt thật và quãng đường chống quẩn. + * 2. Chạy state machine với phản hồi thu được từ cycle TRƯỚC. + * 3. Thi hành output: nhận yêu cầu, đẩy plan, khởi động/tick recovery, chạy controller. + * 4. Đưa lệnh ứng viên qua bộ trọng tài để ra lệnh cuối cùng. + * 5. Báo kết quả cho mission layer nếu state machine yêu cầu. + * + * Phản hồi trễ một cycle là có chủ đích và tường minh: nhờ vậy state machine luôn quyết định trước + * rồi mới gọi cổng, không có vòng "gọi để biết nên gọi gì". + * + * Ở Phase 1 lớp này chạy đồng bộ, một thread, và lập plan ngay trong @ref step. Thread planner + * riêng cùng bộ đệm plan ba lớp thuộc phần nối dây runtime, thêm sau; tách như vậy để phần quyết + * định kiểm được mà không cần thread. + * + * @note Không thread-safe. Chỉ control thread được gọi — đây là thread duy nhất phát cmd_vel. + */ +class ControlLoop +{ +public: + ControlLoop() = default; + + /** + * @brief Nạp cấu hình và các cổng. + * @param[out] error Lý do không cấu hình được; chỉ ghi khi hàm trả false. + */ + bool configure(const ControlLoopConfig& config, const ControlLoopDeps& deps, std::string& error); + + bool initialized() const + { + return initialized_; + } + + /** + * @brief Nhận một yêu cầu mới. + * + * Yêu cầu được xếp vào chỗ chờ và chỉ thực sự bắt đầu ở cycle kế tiếp — đó là chỗ duy nhất + * chuyển từ kIdle sang kPlanning, nên không có đường nào khác để một goal lọt vào lõi. + * + * @return false nếu chưa configure, goal không hợp lệ, hoặc không nạp được planner cho profile. + */ + bool submit(const NavigationRequest& request, std::string& reason); + + void requestPause(); + void requestResume(); + void requestCancel(); + + /** + * @brief Chạy một control cycle. + * @return false khi state machine vừa bước vào state terminal ở cycle này. + */ + bool step(); + + /// @brief Lệnh vận tốc phát ra ở cycle gần nhất. + const robot_geometry_msgs::Twist& lastCommand() const + { + return arbiter_.lastCommand(); + } + + /** + * @brief Mốc thời gian của cycle gần nhất — cũng là thời điểm @ref lastCommand được sinh ra. + * + * Dùng để đóng dấu lệnh vận tốc gửi ra host. Host loại lệnh quá cũ theo dấu này, nên nó phải là + * thời gian ĐỌC TỪ CỔNG ĐỒNG HỒ ở đầu cycle, không phải giờ hệ thống lúc host hỏi: nếu control + * loop treo, dấu thời gian phải đứng yên để host nhìn thấy sự cố đó. + * + * @return Thời gian mặc định (0) khi chưa cycle nào chạy — luôn bị coi là quá hạn. + */ + robot::Time lastCycleTime() const + { + return last_cycle_time_; + } + + NavigationState state() const + { + return state_machine_.state(); + } + + /// @brief Kết quả của yêu cầu vừa kết thúc; chuỗi rỗng nếu chưa có yêu cầu nào kết thúc. + const char* lastOutcome() const; + + bool hasOutcome() const + { + return has_outcome_; + } + + /// @brief Số lần đã báo kết quả — dùng để kiểm bất biến "đúng một lần cho mỗi yêu cầu". + std::size_t outcomeReportCount() const + { + return outcome_report_count_; + } + + const StateMachine& stateMachine() const + { + return state_machine_; + } + + const VelocityArbiter& arbiter() const + { + return arbiter_; + } + + /// @brief Cổng pose đang dùng; nullptr nếu chưa configure. Non-owning, không được cache lại. + PosePort* posePort() const + { + return deps_.pose; + } + + /** + * @brief Cổng controller đang dùng; nullptr nếu chưa configure. Non-owning, không cache lại. + * + * @warning Chỉ được dùng từ **control thread**. Có mặt ở đây để `NavigationServer` đẩy trần vận + * tốc và vận tốc đo được xuống trong `spinOnce()`, chứ không phải để host gọi thẳng. + */ + ControllerPort* controllerPort() const + { + return deps_.controller; + } + + /// @brief Lý do chuyển state gần nhất. Chỉ đổi khi state đổi, nên log được mà không spam. + const char* lastReason() const + { + return last_reason_; + } + + /// @brief Đưa loop về trạng thái ban đầu, giữ nguyên cấu hình và các cổng. + 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 Thu kết quả lập plan bất đồng bộ và quy nó thành @ref planner_feedback_. + * + * Gọi ở **đầu** cycle, trước khi dựng dữ liệu vào cho state machine: state machine tiêu thụ phản + * hồi rồi mới quyết định, nên kết quả phải có mặt trước lúc đó. + */ + void collectPlannerResult(); + + /// @brief Gọi controller và lấy lệnh ứng viên. + void runController(robot_geometry_msgs::Twist& candidate); + + /// @brief Quaternion có chuẩn hoá được không — goal hỏng phải bị từ chối ngay tại cửa. + static bool isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose); + + ControlLoopConfig config_; + ControlLoopDeps deps_; + bool initialized_ = false; + + StateMachine state_machine_; + VelocityArbiter arbiter_; + + NavigationRequest pending_request_; + bool has_pending_request_ = false; + NavigationRequest active_request_; + bool has_active_request_ = false; + + bool pause_requested_ = false; + bool resume_requested_ = false; + bool cancel_requested_ = false; + + /// Phản hồi thu được ở cycle trước, dùng làm dữ liệu vào cho cycle này. + PlannerFeedback planner_feedback_ = PlannerFeedback::kIdle; + ControllerFeedback controller_feedback_ = ControllerFeedback::kIdle; + RecoveryFeedback recovery_feedback_ = RecoveryFeedback::kIdle; + ActionFeedback action_feedback_ = ActionFeedback::kIdle; + + std::vector latest_plan_; + bool planner_running_ = false; + + /** + * Nhãn của yêu cầu đang chạy, cấp cho từng lượt lập plan. + * + * Lập plan mất hàng trăm ms — lâu hơn tuổi thọ của goal sinh ra nó trong trường hợp bị huỷ hay + * thay bằng yêu cầu khác. Kết quả về sau mang nhãn cũ sẽ bị vứt, thay vì được bám theo tới một + * goal không còn ai yêu cầu. + */ + std::uint64_t plan_tag_ = 0; + + robot::Time last_cycle_time_; + bool has_last_cycle_time_ = false; + + robot_geometry_msgs::PoseStamped oscillation_origin_; + bool has_oscillation_origin_ = false; + + bool has_outcome_ = false; + NavigationOutcome last_outcome_ = NavigationOutcome::kFailed; + std::size_t outcome_report_count_ = 0; + + const char* last_reason_ = ""; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_CONTROL_LOOP_H_ diff --git a/include/move_base2/core/navigation_request.h b/include/move_base2/core/navigation_request.h new file mode 100644 index 0000000..738fcd3 --- /dev/null +++ b/include/move_base2/core/navigation_request.h @@ -0,0 +1,114 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — một yêu cầu navigation, gộp mọi kiểu chuyển động về một struct. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_ +#define MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_ + +#include +#include +#include +#include + +#include +#include +#include + +namespace move_base2 +{ + +/** + * @enum MotionProfile + * @brief Kiểu chuyển động của một yêu cầu. + * + * Profile quyết định local planner nào được nạp và global planner nào được swap. Nó là thứ duy nhất + * thực sự khác nhau giữa sáu entry point của contract host cũ (moveTo ×2, dockTo ×2, moveStraightTo, + * rotateTo) — phần còn lại của sáu hàm đó giống hệt nhau. + */ +enum class MotionProfile +{ + kPosition, ///< Di chuyển tới một pose bất kỳ. + kDocking, ///< Ghép nối vào marker (sạc, trạm nạp…). + kGoStraight, ///< Đi thẳng theo trục X của robot. + kRotate ///< Xoay tại chỗ tới hướng đích. +}; + +/// @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. + * + * Đây là kiểu dữ liệu duy nhất đi vào lõi. Mọi nguồn goal — contract host, action server, mission + * layer — đều quy về struct này, nên lõi chỉ có một đường vào và một chỗ để khoá. + */ +struct NavigationRequest +{ + MotionProfile profile = MotionProfile::kPosition; + + /** + * D8: false = yêu cầu chỉ-có-action, không có chặng navigation nào. Runtime bỏ qua + * planning/controlling và vào thẳng thực thi action; @ref goal khi đó không có nghĩa và không + * được validate. Yêu cầu không có goal lẫn action là vi phạm contract và bị từ chối tại cửa. + */ + bool has_goal = true; + + /// 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 + * nội dung — việc đó thuộc ActionHandler phía sau ActionPort. + */ + std::vector actions; + + /// Chỉ dùng cho @ref MotionProfile::kDocking; rỗng với các profile khác. + std::string marker; + + /// 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; + + /** + * Số hiệu chặng do mission layer cấp. 0 = goal trực tiếp, không thuộc mission nào. + * + * Giá trị này được echo nguyên vẹn khi báo kết quả, và là khoá để giữ bất biến "mỗi chặng chỉ + * được báo kết quả đúng một lần". + */ + std::uint64_t mission_sequence_id = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_ diff --git a/include/move_base2/core/navigation_state.h b/include/move_base2/core/navigation_state.h new file mode 100644 index 0000000..536a6d7 --- /dev/null +++ b/include/move_base2/core/navigation_state.h @@ -0,0 +1,61 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — tập state của navigation runtime. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CORE_NAVIGATION_STATE_H_ +#define MOVE_BASE2_CORE_NAVIGATION_STATE_H_ + +namespace move_base2 +{ + +/** + * @enum NavigationState + * @brief State của một yêu cầu navigation. + * + * Ba điểm khác bản runtime thế hệ 1, đều có lý do vận hành: + * + * 1. `kRecovering` thay cho `CLEARING` và là state **có thời lượng** (nhiều control cycle), không + * phải một lời gọi blocking. Bắt buộc như vậy vì recovery thế hệ 2 trả kết quả từng tick và có + * thể phát vận tốc — nghĩa là nó phải chạy cùng nhịp với controller. + * + * 2. `kPaused` và `kCancelling` là state thật, không phải cờ đọc rải rác trong vòng lặp. Nhờ vậy + * đường huỷ chỉ còn một chỗ duy nhất thay vì lặp lại ở nhánh try và nhánh catch. + * + * 3. `kSucceeded` / `kAborted` / `kCancelled` là state, không phải `return` giữa hàm. Đây là thứ + * giữ được bất biến "báo kết quả đúng một lần cho mỗi yêu cầu". + * + * 4. `kExecutingActions` (D8): runtime điều phối trọn một mission — tới goal xong còn chạy nốt + * các action của mission (nâng/hạ, sạc, chờ…) rồi mới báo kết quả. Trong state này KHÔNG ai + * được phát vận tốc: action cần chuyển động phải được mô hình hoá thành motion profile của + * navigation, không phải action. + */ +enum class NavigationState +{ + kIdle, ///< Không có yêu cầu nào đang chạy. Vận tốc = 0. + kPlanning, ///< Đang chờ global planner ra plan. Vận tốc = 0. + kControlling, ///< Đang bám plan. Local planner là nguồn vận tốc. + kRecovering, ///< Đang chạy một recovery behavior. Recovery là nguồn vận tốc. + kExecutingActions, ///< Đang chạy action của mission tại chỗ (D8). Vận tốc = 0. + kPaused, ///< Tạm dừng theo yêu cầu. Vận tốc = 0. + kCancelling, ///< Đang giảm tốc để huỷ. Vận tốc = 0, chờ robot dừng hẳn. + kSucceeded, ///< Terminal: đạt goal và mọi action đã xong. + kAborted, ///< Terminal: hết cách, không đạt được goal hoặc action thất bại. + kCancelled ///< Terminal: đã huỷ theo yêu cầu và robot đã dừng. +}; + +/// @brief Tên state dạng chuỗi, dùng cho log và cho phần assert chuỗi state trong test. +const char* toString(NavigationState state); + +/// @brief State kết thúc một yêu cầu (kSucceeded/kAborted/kCancelled). +bool isTerminal(NavigationState state); + +/// @brief State mà theo thiết kế KHÔNG được phép có vận tốc khác 0 — bất biến an toàn. +bool mustBeStopped(NavigationState state); + +} // namespace move_base2 + +#endif // MOVE_BASE2_CORE_NAVIGATION_STATE_H_ diff --git a/include/move_base2/core/state_machine.h b/include/move_base2/core/state_machine.h new file mode 100644 index 0000000..e63d01a --- /dev/null +++ b/include/move_base2/core/state_machine.h @@ -0,0 +1,327 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — state machine của navigation runtime. Logic thuần, không I/O. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CORE_STATE_MACHINE_H_ +#define MOVE_BASE2_CORE_STATE_MACHINE_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +namespace move_base2 +{ + +/// @brief Tin từ global planner cho cycle này. +enum class PlannerFeedback +{ + kIdle, ///< Chưa có tin gì mới kể từ cycle trước. + kBusy, ///< Đang lập plan, chưa có kết quả. + kPlanReady, ///< Có plan mới, không rỗng, sẵn sàng đẩy xuống controller. + kFailed ///< Lần lập plan gần nhất thất bại. +}; + +/// @brief Tin từ local planner cho cycle này. +enum class ControllerFeedback +{ + kIdle, ///< Chưa chạy controller ở cycle này. + kCommandValid, ///< Sinh được lệnh vận tốc hợp lệ. + kNoValidCommand, ///< Không sinh được lệnh hợp lệ. + kGoalReached ///< Đã tới đích trong sai số cho phép. +}; + +/// @brief Tin từ recovery cho cycle này. +enum class RecoveryFeedback +{ + kIdle, + kRunning, + kSucceeded, + kFailed +}; + +/// @brief Tin từ action đang chạy cho cycle này (D8). +enum class ActionFeedback +{ + kIdle, ///< Chưa chạy action ở cycle này. + kRunning, + kSucceeded, ///< Action HIỆN TẠI xong; còn action kế tiếp hay không do state machine quyết. + kFailed +}; + +/** + * @struct StateMachineConfig + * @brief Tham số điều khiển hành vi chuyển state. + * + * Theo quy ước của repo, mọi tham số đều có default trong struct, có @ref validate kiểm miền giá + * trị, và có ghi đơn vị ngay tại khai báo. + */ +struct StateMachineConfig +{ + /// [s] Thời gian tối đa được ở trạng thái chưa có plan hợp lệ trước khi vào recovery. <= 0 = tắt. + double planner_patience = 5.0; + + /// [s] Thời gian tối đa không sinh được lệnh hợp lệ trước khi vào recovery. <= 0 = tắt. + double controller_patience = 15.0; + + /// [s] Thời gian tối đa được quẩn trong bán kính @ref oscillation_distance. <= 0 = tắt. + double oscillation_timeout = 0.0; + + /** + * [s] Trần thời gian cho MỘT action, tính từ lúc start. <= 0 = tắt (mặc định). + * + * Đây là lưới an toàn CUỐI CÙNG, không phải cơ chế timeout chính: mỗi ActionHandler phải tự + * timeout theo hiểu biết thiết bị của nó ("nâng kệ quá 20 s là bất thường" vs "sạc 30 phút là + * bình thường"), và mission layer còn `mission_timeout` cho cả chặng. Chỉ bật giá trị này khi + * deployment biết chắc mọi action đều ngắn hơn một trần chung. Quá hạn -> cancel action + + * ABORTED. + */ + double action_patience = 0.0; + + /// [m] Đi được quá khoảng này thì coi như không còn quẩn, đồng hồ oscillation reset. + double oscillation_distance = 0.5; + + /// Số lần lập plan hỏng liên tiếp tối đa. < 0 = không giới hạn (chỉ chặn bằng planner_patience). + int max_planning_retries = -1; + + /// Số recovery behavior đã nạp được. 0 = không có đường phục hồi, lỗi là ABORTED ngay. + std::size_t recovery_behavior_count = 0; + + /// 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; + + /** + * @brief Kiểm miền giá trị. + * @param[out] error Mô tả tham số sai; chỉ được ghi khi hàm trả false. + */ + bool validate(std::string& error) const; + + /// @brief Kết xuất cấu hình thành text nhiều dòng, để log một lần lúc khởi tạo. + std::string describe() const; +}; + +/** + * @struct StateMachineInput + * @brief Toàn bộ dữ liệu vào của một lần chuyển state. + * + * Cố ý không chứa costmap, tf, hay con trỏ tới bất kỳ thành phần nào: state machine phải kiểm được + * bằng bảng, không cần dựng runtime. + */ +struct StateMachineInput +{ + robot::Time now; ///< Thời điểm của cycle này. + + bool has_pending_request = false; ///< Có yêu cầu mới đang chờ được nhận. + + /// D8 — hình dạng của yêu cầu đang chờ; chỉ có nghĩa khi @ref has_pending_request. State machine + /// chốt lại hai giá trị này tại cycle nhận yêu cầu, các cycle sau không đọc nữa. + bool pending_request_has_goal = true; + std::size_t pending_request_action_count = 0; + + bool pause_requested = false; + bool resume_requested = false; + bool cancel_requested = false; + + PlannerFeedback planner = PlannerFeedback::kIdle; + ControllerFeedback controller = ControllerFeedback::kIdle; + RecoveryFeedback recovery = RecoveryFeedback::kIdle; + ActionFeedback action = ActionFeedback::kIdle; + + /** + * Lấy được pose robot hay không. + * + * false nghĩa là TF thiếu hoặc quá hạn. Khi đó state machine ép nguồn vận tốc về kNone ở MỌI + * state: không biết robot ở đâu thì không được cho nó chạy, kể cả đang recovery. Các đồng hồ + * kiên nhẫn vẫn chạy, nên mất TF kéo dài cuối cùng vẫn dẫn tới recovery rồi ABORTED thay vì treo. + */ + bool pose_available = true; + + /// Robot đã dừng hẳn chưa (|v| <= ngưỡng). Dùng để rời kCancelling. + bool robot_stopped = true; + + /** + * Họ output của behavior sẽ chạy / đang chạy ở cycle này. + * + * Bên gọi lấy từ `RecoveryPort::outputKind(nextRecoveryIndex())` — cùng một chỉ số dùng cho cả + * cycle khởi động lẫn các cycle tick, vì chỉ số chỉ tăng khi behavior kết thúc. + * + * State machine dùng nó để KHÔNG trao quyền phát vận tốc cho behavior không lái robot (đợi, xoá + * costmap). Chỉ có nghĩa khi state là kRecovering. + */ + RecoveryOutputKind active_recovery_output = RecoveryOutputKind::kVelocity; + + /// [m] Quãng đường đi được kể từ mốc oscillation gần nhất. + double travelled_since_oscillation_reset = 0.0; +}; + +/** + * @struct StateMachineOutput + * @brief Việc cần làm sau một lần chuyển state. + * + * Đây là danh sách hành động, không phải lời gọi: state machine không tự thi hành gì. Nhờ vậy test + * kiểm được "đáng lẽ phải làm gì" tách rời khỏi "làm thế nào". + */ +struct StateMachineOutput +{ + NavigationState state = NavigationState::kIdle; + bool state_changed = false; + + bool accept_request = false; ///< Lấy yêu cầu đang chờ ra khỏi hàng đợi và coi là yêu cầu hiện tại. + bool start_planner = false; ///< Yêu cầu planner bắt đầu/tiếp tục lập plan. + bool stop_planner = false; ///< Yêu cầu planner ngừng. + bool apply_plan = false; ///< Đẩy plan mới nhất xuống controller. + bool run_controller = false; ///< Gọi controller ở cycle này. + + bool start_recovery = false; ///< Khởi động behavior @ref recovery_index với @ref recovery_trigger. + bool tick_recovery = false; ///< Gọi update() của behavior đang chạy. + bool cancel_recovery = false; ///< Yêu cầu behavior đang chạy dừng. + + bool start_action = false; ///< D8: khởi động action thứ @ref action_index của yêu cầu hiện tại. + bool tick_action = false; ///< D8: gọi update() của action đang chạy. + bool cancel_action = false; ///< D8: yêu cầu action đang chạy dừng an toàn. + + /// Chỉ có nghĩa khi @ref start_action — action nào trong danh sách của yêu cầu cần khởi động. + std::size_t action_index = 0; + + bool reset_oscillation_origin = false; ///< Đặt lại mốc đo quãng đường chống quẩn về pose hiện tại. + + bool report_outcome = false; ///< Báo kết quả chặng — đúng một lần. + NavigationOutcome outcome = NavigationOutcome::kFailed; + + /// Chỉ có nghĩa khi @ref start_recovery — behavior nào cần khởi động và vì lý do gì. + std::size_t recovery_index = 0; + RecoveryTrigger recovery_trigger = RecoveryTrigger::kPlanningFailed; + + /// Nguồn được phép phát vận tốc ở cycle này. Luôn có đúng một nguồn. + VelocitySource velocity_source = VelocitySource::kNone; + + /// Lý do chuyển state, chuỗi hằng. Chỉ log khi @ref state_changed để không spam control loop. + const char* reason = ""; +}; + +/** + * @class StateMachine + * @brief Bộ chuyển state của navigation runtime. + * + * Logic thuần: không đụng costmap, không đụng tf, không log, không cấp phát trong @ref update. + * Toàn bộ bảng chuyển được mô tả trong `docs/STATE_MACHINE.md` và bảng đó là nguồn chuẩn. + * + * Bất biến được giữ: + * - Mỗi cycle có đúng MỘT nguồn vận tốc (@ref StateMachineOutput::velocity_source). + * - @ref StateMachineOutput::run_controller, @ref StateMachineOutput::tick_recovery và + * @ref StateMachineOutput::tick_action đôi một không bao giờ cùng true. + * - @ref StateMachineOutput::report_outcome bật đúng một lần cho mỗi yêu cầu, tại cycle bước vào + * state terminal — kể cả khi yêu cầu có action: kết quả chỉ được báo sau action cuối (D8). + * - Ở state phải dừng (@ref mustBeStopped) thì velocity_source luôn là kNone; kExecutingActions + * thuộc nhóm này — action không bao giờ đi kèm vận tốc. + * + * @note Không thread-safe. Chỉ control thread được gọi. + */ +class StateMachine +{ +public: + StateMachine() = default; + + /** + * @brief Nạp cấu hình và đưa state machine về kIdle. + * @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false. + * @return false nếu cấu hình không hợp lệ — bên gọi KHÔNG được chạy tiếp với cấu hình hỏng. + */ + bool configure(const StateMachineConfig& config, std::string& error); + + /// @brief Đã configure thành công hay chưa. @ref update trả kIdle nếu chưa. + bool initialized() const + { + return initialized_; + } + + /// @brief Chạy một cycle. + StateMachineOutput update(const StateMachineInput& input); + + /// @brief Đưa về kIdle, xoá mọi bộ đếm và đồng hồ. Dùng khi khởi động lại runtime. + void reset(); + + NavigationState state() const + { + return state_; + } + + const StateMachineConfig& config() const + { + return config_; + } + + /// @brief Chỉ số behavior sẽ chạy ở lần vào recovery kế tiếp. Dùng để assert trong test. + std::size_t nextRecoveryIndex() const + { + return recovery_index_; + } + + /// @brief Chỉ số action đang chạy / sẽ chạy kế tiếp của yêu cầu hiện tại. Dùng để assert trong test. + std::size_t currentActionIndex() const + { + return action_index_; + } + + /// @brief Số lần lập plan hỏng liên tiếp tính từ lần vào kPlanning gần nhất. + int planningRetries() const + { + return planning_retries_; + } + + /// @brief Thời gian [s] đã ở trong state hiện tại, tính tới @p now. + double secondsInState(const robot::Time& now) const; + +private: + /// @brief Chuyển sang @p next và ghi nhận thời điểm vào state. + void enter(NavigationState next, const robot::Time& now, const char* reason, + StateMachineOutput& out); + + /// @brief Bắt đầu một chu kỳ lập plan: reset đồng hồ kiên nhẫn và bộ đếm retry. + void beginPlanningCycle(const robot::Time& now); + + /** + * @brief Vào recovery nếu còn behavior, ngược lại kết thúc bằng kAborted. + * @param trigger Lý do vào recovery. + */ + void escalateToRecovery(RecoveryTrigger trigger, const robot::Time& now, const char* reason, + StateMachineOutput& out); + + /// @brief Vào state terminal và bật cờ báo kết quả đúng một lần. + void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now, + const char* reason, StateMachineOutput& out); + + StateMachineConfig config_; + bool initialized_ = false; + + NavigationState state_ = NavigationState::kIdle; + /// State để quay về sau kPaused. Không bao giờ là kPaused hay state terminal. + NavigationState state_before_pause_ = NavigationState::kIdle; + + robot::Time state_entered_at_; + robot::Time last_valid_plan_; ///< Mốc đo planner_patience. + robot::Time last_valid_control_; ///< Mốc đo controller_patience. + robot::Time last_oscillation_reset_; + + std::size_t recovery_index_ = 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. + bool request_has_goal_ = true; + std::size_t action_count_ = 0; + std::size_t action_index_ = 0; + robot::Time action_started_at_; ///< Mốc đo action_patience; đặt lại mỗi lần start action và khi resume. +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_CORE_STATE_MACHINE_H_ diff --git a/include/move_base2/core/velocity_arbiter.h b/include/move_base2/core/velocity_arbiter.h new file mode 100644 index 0000000..0379b5e --- /dev/null +++ b/include/move_base2/core/velocity_arbiter.h @@ -0,0 +1,180 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — bộ trọng tài vận tốc: ai được phát lệnh, và lệnh đó có an toàn không. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_CORE_VELOCITY_ARBITER_H_ +#define MOVE_BASE2_CORE_VELOCITY_ARBITER_H_ + +#include +#include + +#include + +namespace move_base2 +{ + +/** + * @enum VelocitySource + * @brief Nguồn vận tốc hợp lệ tại một thời điểm. LUÔN chỉ có một. + */ +enum class VelocitySource +{ + kNone, ///< Không ai được phát — lệnh ra ngoài là 0. + kController, ///< Local planner. + kRecovery ///< Recovery behavior đang chạy. +}; + +const char* toString(VelocitySource source); + +/** + * @struct VelocityLimits + * @brief Giới hạn động học áp cho mọi lệnh trước khi ra khỏi runtime. + * + * Đây là hàng rào cuối cùng, không phải bộ điều khiển: local planner đã có giới hạn riêng, nhưng + * runtime không được tin bất kỳ nguồn nào — kể cả recovery behavior nạp từ plugin ngoài. + */ +struct VelocityLimits +{ + double max_vel_x = 0.5; ///< [m/s] trần tốc độ tiến (dương). + double min_vel_x = -0.2; ///< [m/s] trần tốc độ lùi (ÂM). 0 = cấm lùi. + double max_vel_theta = 1.0; ///< [rad/s] trần tốc độ quay tuyệt đối. + double max_accel_x = 1.0; ///< [m/s^2] trần biến thiên tốc độ dài, áp cho cả tăng và giảm. + double max_accel_theta = 2.0; ///< [rad/s^2] trần biến thiên tốc độ quay. + + /// [m/s] và [rad/s] — dưới ngưỡng này coi như đã dừng. + double zero_velocity_epsilon = 1e-3; + + /** + * @brief Kiểm miền giá trị. + * @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false. + */ + bool validate(std::string& error) const; + + /// @brief Kết xuất cấu hình thành text nhiều dòng, để log một lần lúc khởi tạo. + std::string describe() const; +}; + +/** + * @class VelocityArbiter + * @brief Chốt chặn duy nhất giữa các nguồn lệnh và cmd_vel ra ngoài. + * + * Ba quy tắc, đều được viết thành assert + test chứ không chỉ là comment: + * + * 1. **Nguồn kNone phát lệnh 0 tức thì**, không giảm tốc dần. Lệnh vận tốc bị chốt lại ở tầng + * dưới, nên nếu control loop dừng giữa lúc đang giảm tốc thì lệnh khác 0 cuối cùng vẫn còn hiệu + * lực và robot chạy tiếp. Việc giảm tốc theo động học thuộc về bộ điều khiển bánh xe. + * + * 2. **Mọi lệnh phải qua sanitize.** NaN/Inf bị chặn thành 0 và được đếm lại; giá trị vượt trần bị + * clamp theo @ref VelocityLimits; biến thiên bị clamp theo gia tốc và dt THẬT của cycle, không + * phải chu kỳ danh nghĩa trong config. + * + * 3. **Đổi nguồn bắt buộc chèn ít nhất một cycle vận tốc 0.** Controller và recovery giữ trạng + * thái gia tốc riêng; chuyển thẳng từ nguồn này sang nguồn kia gây giật. Ở thế hệ 1 vấn đề này + * không tồn tại vì recovery chạy blocking và không phát vận tốc; ở thế hệ 2 thì có. + * + * @note Không thread-safe. Chỉ control thread — thread duy nhất được phát cmd_vel — gọi lớp này. + */ +class VelocityArbiter +{ +public: + VelocityArbiter() = default; + + /** + * @brief Nạp giới hạn và đưa bộ trọng tài về trạng thái dừng. + * @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false. + */ + bool configure(const VelocityLimits& limits, std::string& error); + + bool initialized() const + { + return initialized_; + } + + /** + * @brief Quyết định lệnh thực sự phát ra ở cycle này. + * @param source Nguồn được state machine cho phép. + * @param candidate Lệnh nguồn đó đề nghị. Bỏ qua khi @p source là kNone. + * @param dt [s] Khoảng thời gian THẬT từ cycle trước. <= 0 thì bỏ qua giới hạn gia tốc. + * @return Lệnh an toàn để gửi ra ngoài. + */ + robot_geometry_msgs::Twist arbitrate(VelocitySource source, + const robot_geometry_msgs::Twist& candidate, double dt); + + /// @brief Ép về 0 ngay lập tức, bỏ qua giới hạn gia tốc. Dùng cho dừng khẩn. + robot_geometry_msgs::Twist emergencyStop(); + + /// @brief Lệnh đã phát ở cycle gần nhất. + const robot_geometry_msgs::Twist& lastCommand() const + { + return last_command_; + } + + /// @brief Nguồn đang được công nhận sau lần @ref arbitrate gần nhất. + VelocitySource activeSource() const + { + return active_source_; + } + + /// @brief Lệnh gần nhất có coi như đã dừng hay không (theo zero_velocity_epsilon). + bool stopped() const; + + /// @brief Số lần chặn được NaN/Inf. Khác 0 nghĩa là có nguồn đang trả dữ liệu hỏng. + std::size_t nonFiniteRejections() const + { + return non_finite_rejections_; + } + + /// @brief Số lần phải clamp vì vượt trần vận tốc. + std::size_t velocityClamps() const + { + return velocity_clamps_; + } + + /// @brief Số lần phải clamp vì vượt trần gia tốc. + std::size_t accelerationClamps() const + { + return acceleration_clamps_; + } + + /// @brief Số cycle 0 đã chèn vào khi đổi nguồn. + std::size_t handoverCycles() const + { + return handover_cycles_; + } + + /// @brief Về trạng thái dừng, xoá mọi bộ đếm và lịch sử lệnh. + void reset(); + + const VelocityLimits& limits() const + { + return limits_; + } + +private: + /// @brief Chặn NaN/Inf và clamp theo trần vận tốc. Trả về lệnh đã làm sạch. + robot_geometry_msgs::Twist sanitize(const robot_geometry_msgs::Twist& candidate); + + /// @brief Clamp biến thiên so với lệnh trước theo gia tốc và @p dt thật. + robot_geometry_msgs::Twist limitAcceleration(const robot_geometry_msgs::Twist& target, double dt); + + static robot_geometry_msgs::Twist zeroTwist(); + + VelocityLimits limits_; + bool initialized_ = false; + + VelocitySource active_source_ = VelocitySource::kNone; + robot_geometry_msgs::Twist last_command_; + + std::size_t non_finite_rejections_ = 0; + std::size_t velocity_clamps_ = 0; + std::size_t acceleration_clamps_ = 0; + std::size_t handover_cycles_ = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_CORE_VELOCITY_ARBITER_H_ diff --git a/include/move_base2/io/sensor_gateway.h b/include/move_base2/io/sensor_gateway.h new file mode 100644 index 0000000..c555b82 --- /dev/null +++ b/include/move_base2/io/sensor_gateway.h @@ -0,0 +1,206 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cửa vào dữ liệu cảm biến: từ contract host tới các layer costmap. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_IO_SENSOR_GATEWAY_H_ +#define MOVE_BASE2_IO_SENSOR_GATEWAY_H_ + +#include +#include +#include + +#include +#include +#include +#include +#include + +// Chỉ cần con trỏ và một thành viên pimpl: định nghĩa thật nằm trong .cpp, nên file này không kéo +// robot_costmap_2d và laser_filter vào mọi translation unit chạm tới nó. +namespace robot_costmap_2d +{ +class LayeredCostmap; +} +namespace laser_filter +{ +class LaserScanSOR; +} + +namespace move_base2 +{ + +/** + * @struct SensorGatewayConfig + * @brief Tham số của đường vào cảm biến. + */ +struct SensorGatewayConfig +{ + /** + * @brief Bật lọc statistical-outlier-removal cho LaserScan trước khi đưa vào costmap. + * + * Default **tắt**, và đó là quyết định có chủ đích. Bản `move_base` cũ chỉ lọc ở nhánh biên dịch + * không-ROS (`#ifndef BUILD_WITH_ROS`), nên cùng một contract mà hai host nhìn thấy hai tập vật + * cản khác nhau. Ở đây hành vi được hợp nhất thành một khoá YAML duy nhất; tắt theo mặc định giữ + * đúng thứ robot đang chạy dưới host ROS hôm nay. Bật một bộ lọc theo mặc định là âm thầm xoá bớt + * điểm vật cản — không phải thứ nên tự xảy ra. + */ + bool laser_sor_enabled = false; + + /// [điểm] Số láng giềng gần nhất dùng để ước lượng khoảng cách trung bình. Chỉ dùng khi bật lọc. + int laser_sor_mean_k = 10; + + /// [-] Ngưỡng outlier = mean + hệ_số * stddev. Chỉ dùng khi bật lọc. + double laser_sor_stddev_mul = 1.0; + + /** + * @brief Kiểm miền giá trị. + * @param[out] error Mô tả tham số sai đầu tiên; chỉ ghi khi hàm trả false. + */ + bool validate(std::string& error) const; + + /// @brief Kết xuất nhiều dòng để log đúng một lần lúc khởi tạo. + std::string describe() const; +}; + +/** + * @struct SensorGatewayStats + * @brief Đếm số phận của các mẫu đã đi qua cổng. + * + * Bản cũ bỏ mẫu **im lặng** ở ba chỗ khác nhau (chưa có costmap, layer tắt, sai tên topic) nên + * "costmap không thấy vật cản" không có cách nào chẩn đoán ngoài việc đọc lại code. Các bộ đếm này + * tồn tại để câu hỏi đó trả lời được bằng số. + */ +struct SensorGatewayStats +{ + /// Số lần một layer thực sự nhận được dữ liệu (đếm theo cặp mẫu-layer). + std::size_t delivered = 0; + + /// Số mẫu bị bỏ vì chưa có costmap nào được gắn. + std::size_t dropped_no_costmap = 0; + + /// Số lần bỏ qua một layer vì layer đó đang tắt (`enabled: false`). + std::size_t skipped_disabled = 0; + + /// Số lần `handleImpl` của một layer ném exception. + std::size_t layer_exceptions = 0; +}; + +/** + * @class SensorGateway + * @brief Đưa dữ liệu cảm biến từ contract host vào đúng các layer của hai costmap. + * + * Đây là file **duy nhất** trong `move_base2` biết tới `robot_costmap_2d`. Lõi quyết định + * (`StateMachine`, `ControlLoop`) không nhìn thấy costmap, và đó là thứ giữ cho lõi kiểm được bằng + * bảng thay vì phải dựng một costmap thật. + * + * ## Ba contract ẩn phải giữ đúng + * + * 1. **Kiểu phải khớp chính xác.** `Layer::dataCallBack` xoá kiểu về `void*` + `std::type_info`; + * layer so sánh bằng `typeid`. Sai kiểu không gây lỗi biên dịch mà rơi im lặng — vì vậy cổng này + * phơi ra các hàm push có kiểu cụ thể chứ không phải một template mở. + * Riêng depth camera phải truyền đúng dạng `ConstPtr`, không phải giá trị. + * 2. **`name` là khoá topic, không phải nhãn.** Layer tự lọc lần hai: `StaticLayer` so với + * `map_topic`, `ObstacleLayer` so với `topic` của từng observation source trong YAML. Tên sai + * một chữ là mất hẳn một cảm biến, không có cảnh báo nào. + * 3. **Thứ tự và quyền sở hữu.** Con trỏ `LayeredCostmap` là **non-owning**; chủ sở hữu là + * `NavigationServer`. Cổng này không cache `Costmap2D*` bên trong. + * + * ## Khác biệt có chủ đích so với `move_base` cũ + * + * - **Lọc layer chỉ theo `getType()`.** Bản cũ dùng `getType() == type || getName() == name`. Vế + * tên là code chết trong thực tế (tên layer là `obstacles`/`inflation`, tên sensor là + * `/b_scan`/`/map` — hai tập không giao nhau) nhưng lại là một cái bẫy: đặt tên một layer trùng + * tên topic sẽ đẩy dữ liệu vào layer sai, và `InflationLayer::handleImpl` chỉ biết log error nên + * sẽ spam ở đúng tần số cảm biến. + * - **Bỏ qua layer đang tắt.** `ObstacleLayer::handleImpl` tự thoát ngay khi `enabled_` false, nên + * bản cũ làm đủ việc rồi vứt. Ở đây bỏ sớm và **đếm lại**. + * - **try/catch quanh từng layer**, không phải quanh cả vòng lặp: một layer ném exception không + * được làm các layer sau đó mất luôn mẫu dữ liệu đó. + * + * @note Không thread-safe. Người gọi (`NavigationServer`) chịu trách nhiệm tuần tự hoá, và **không + * được giữ lock dữ liệu của mình trong lúc gọi**: `StaticLayer::incomingMap` có thể gọi + * `LayeredCostmap::resizeMap`, hàm này chờ mutex master costmap và có thể đứng trọn một chu + * kỳ `updateMap`. + */ +class SensorGateway +{ +public: + SensorGateway(); + ~SensorGateway(); + + SensorGateway(const SensorGateway&) = delete; + SensorGateway& operator=(const SensorGateway&) = delete; + + /** + * @brief Nạp cấu hình. + * @param[out] error Lý do không cấu hình được; chỉ ghi khi hàm trả false. + */ + bool configure(const SensorGatewayConfig& config, std::string& error); + + /** + * @brief Gắn hai costmap đích. Cả hai đều **non-owning** và đều được phép null. + * + * Null nghĩa là chưa dựng costmap đó — mẫu tới sẽ bị bỏ nhưng được **đếm** và log một lần, thay + * cho kiểu `if (!costmap) return;` im lặng của bản cũ. + * + * Gọi lại hàm này ghi đè con trỏ cũ; dùng khi costmap bị dựng lại. + */ + void attach(robot_costmap_2d::LayeredCostmap* global, robot_costmap_2d::LayeredCostmap* local); + + /// @brief Đã có ít nhất một costmap đích hay chưa. + bool attached() const; + + /** + * @brief Áp bộ lọc đã cấu hình lên một mẫu laser. + * + * Tách khỏi @ref pushLaserScan để người gọi **cất đúng bản đã lọc** — bản cũ cũng lưu bản đã lọc, + * và nếu getter của contract host trả bản thô trong khi costmap thấy bản lọc thì hai nguồn sự + * thật sẽ lệch nhau. + * + * @return Chính @p scan khi lọc đang tắt. + */ + robot_sensor_msgs::LaserScan prepareLaserScan(const robot_sensor_msgs::LaserScan& scan) const; + + // ================================================================================================ + // Đẩy vào costmap. Tên tham số `name` chính là khoá topic mà layer sẽ so — xem contract ẩn #2. + // ================================================================================================ + + void pushStaticMap(const std::string& name, const robot_nav_msgs::OccupancyGrid& map); + void pushLaserScan(const std::string& name, const robot_sensor_msgs::LaserScan& scan); + void pushPointCloud(const std::string& name, const robot_sensor_msgs::PointCloud& cloud); + void pushPointCloud2(const std::string& name, const robot_sensor_msgs::PointCloud2& cloud); + void pushDepthCameraData(const std::string& topic, + const robot_sensor_msgs::DepthCameraData::ConstPtr& data); + + const SensorGatewayStats& stats() const + { + return stats_; + } + + void resetStats() + { + stats_ = SensorGatewayStats(); + } + +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; + + SensorGatewayConfig config_; + bool configured_ = false; + + robot_costmap_2d::LayeredCostmap* global_costmap_ = nullptr; + robot_costmap_2d::LayeredCostmap* local_costmap_ = nullptr; + + std::unique_ptr laser_sor_; + + SensorGatewayStats stats_; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_IO_SENSOR_GATEWAY_H_ diff --git a/include/move_base2/navigation_server.h b/include/move_base2/navigation_server.h new file mode 100644 index 0000000..3f851ac --- /dev/null +++ b/include/move_base2/navigation_server.h @@ -0,0 +1,241 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — facade hiện thực contract host BaseNavigation. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_NAVIGATION_SERVER_H_ +#define MOVE_BASE2_NAVIGATION_SERVER_H_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace move_base2 +{ + +/** + * @class NavigationServer + * @brief Lớp biên duy nhất giữa host và lõi navigation. + * + * Nhiệm vụ đúng ba việc, không hơn: + * 1. Quy sáu entry point di chuyển của contract host về một @ref NavigationRequest duy nhất. + * 2. Nhận dữ liệu sensor từ host: cất giữ, rồi chuyển tiếp cho @ref SensorGateway. + * 3. Kết xuất trạng thái lõi ra đúng kiểu mà host mong đợi. + * + * Contract `robot::move_base_core::BaseNavigation` được giữ **nguyên vẹn từng chữ ký**: ba host + * đang phụ thuộc vào nó (bộ điều khiển phía ROS, C API cho .NET, và bản chạy độc lập). Đổi contract + * đồng nghĩa với sửa cả phần binding sang ngôn ngữ khác. + * + * @note Phần nối dây tới planner/controller/recovery thật chưa có, nên @ref initialize chưa dựng + * được các cổng runtime. Test và phần nối dây tự bơm cổng vào qua @ref configureLoop, và bơm + * costmap vào qua @ref attachCostmaps. Chưa gắn costmap thì dữ liệu sensor vẫn được cất giữ + * nhưng không tới được layer nào — @ref SensorGatewayStats::dropped_no_costmap đếm lại số đó. + */ +class NavigationServer : public robot::move_base_core::BaseNavigation +{ +public: + NavigationServer(); + ~NavigationServer() override; + + // ============================================================================================== + // Cấu hình lõi — không thuộc contract host, dùng cho phần nối dây và cho test. + // ============================================================================================== + + /** + * @brief Nạp cấu hình và các cổng cho control loop. + * @param[out] error Lý do không cấu hình được. + */ + bool configureLoop(const ControlLoopConfig& config, const ControlLoopDeps& deps, + std::string& error); + + /// @brief Chạy một control cycle. @return false khi yêu cầu hiện tại vừa kết thúc. + bool spinOnce(); + + const ControlLoop& loop() const + { + return loop_; + } + + ControlLoop& loop() + { + return loop_; + } + + /// @brief Lý do từ chối gần nhất của một lời gọi di chuyển; chuỗi rỗng nếu chưa từ chối lần nào. + const std::string& lastRejectReason() const + { + return last_reject_reason_; + } + + /** + * @brief Nạp cấu hình cho đường vào cảm biến. + * + * Không bắt buộc: chưa gọi thì cổng chạy với default (lọc laser tắt). + * + * @param[out] error Lý do không cấu hình được. + */ + bool configureSensors(const SensorGatewayConfig& config, std::string& error); + + /** + * @brief Gắn hai costmap đích cho dữ liệu cảm biến, rồi phát lại các static map đã nhận. + * + * Phát lại là bắt buộc, không phải tiện ích: dữ liệu tới **trước** khi costmap tồn tại sẽ bị bỏ, + * và trong thực tế `/map` gần như luôn tới trước — bản `move_base` cũ phải bù bằng cặp biến public + * `map_save_`/`map_name_save_` mà host tự gán. Ở đây nguồn phát lại là chính @c static_maps_ đã + * cất, cộng thêm `map_save_` nếu host có dùng đường cũ đó. + * + * Cả hai con trỏ **non-owning** và được phép null. Gọi lại được khi costmap bị dựng lại. + * + * @warning Phải gọi từ thread host và **không** song song với các hàm `add*` — @ref SensorGateway + * không thread-safe. Trong thực tế đây là bước khởi tạo, chạy trước khi sensor bắt đầu. + */ + void attachCostmaps(robot_costmap_2d::LayeredCostmap* global, + robot_costmap_2d::LayeredCostmap* local); + + /// @brief Cổng cảm biến — dùng để đọc bộ đếm chẩn đoán. + const SensorGateway& sensors() const + { + return sensors_; + } + + // ============================================================================================== + // robot::move_base_core::BaseNavigation + // ============================================================================================== + + void initialize(robot::TFListenerPtr tf) override; + + void setRobotFootprint(const std::vector& fprt) override; + std::vector getRobotFootprint() override; + + void addStaticMap(const std::string& map_name, robot_nav_msgs::OccupancyGrid map) override; + void addLaserScan(const std::string& laser_scan_name, + robot_sensor_msgs::LaserScan laser_scan) override; + void addPointCloud(const std::string& point_cloud_name, + robot_sensor_msgs::PointCloud point_cloud) override; + void addPointCloud2(const std::string& point_cloud2_name, + robot_sensor_msgs::PointCloud2 point_cloud2) override; + void addDepthCameraData(const std::string& topic, + robot_sensor_msgs::DepthCameraData::ConstPtr data) override; + + robot_nav_msgs::OccupancyGrid getStaticMap(const std::string& map_name) override; + robot_sensor_msgs::LaserScan getLaserScan(const std::string& laser_scan_name) override; + robot_sensor_msgs::PointCloud getPointCloud(const std::string& point_cloud_name) override; + robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string& point_cloud2_name) override; + + std::map getAllStaticMaps() override; + std::map getAllLaserScans() override; + std::map getAllPointClouds() override; + std::map getAllPointCloud2s() override; + + bool removeStaticMap(const std::string& map_name) override; + bool removeLaserScan(const std::string& laser_scan_name) override; + bool removePointCloud(const std::string& point_cloud_name) override; + bool removePointCloud2(const std::string& point_cloud2_name) override; + + bool removeAllStaticMaps() override; + bool removeAllLaserScans() override; + bool removeAllPointClouds() override; + bool removeAllPointCloud2s() override; + bool removeAllData() override; + + void addOdometry(const std::string& odometry_name, robot_nav_msgs::Odometry odometry) override; + + bool moveTo(const robot_geometry_msgs::PoseStamped& goal, double xy_goal_tolerance, + double yaw_goal_tolerance) override; + bool moveTo(const robot_protocol_msgs::Order& msg, const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double yaw_goal_tolerance) override; + bool dockTo(const std::string& maker, const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double yaw_goal_tolerance) override; + bool dockTo(const robot_protocol_msgs::Order& msg, const std::string& marker, + const robot_geometry_msgs::PoseStamped& goal, double xy_goal_tolerance, + double yaw_goal_tolerance) override; + bool moveStraightTo(const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance) override; + bool rotateTo(const robot_geometry_msgs::PoseStamped& goal, double yaw_goal_tolerance) override; + + void pause() override; + void resume() override; + void cancel() override; + + bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override; + bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override; + + bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) override; + bool getRobotPose(robot_geometry_msgs::Pose2D& pose) override; + + robot_nav_2d_msgs::Twist2DStamped getTwist() override; + + robot::move_base_core::NavFeedback* getFeedback() override; + robot::move_base_core::PlannerDataOutput getGlobalData() override; + robot::move_base_core::PlannerDataOutput getLocalData() override; + +private: + /// @brief Đường vào duy nhất của mọi lệnh di chuyển. Sáu entry point host chỉ dựng struct rồi gọi. + bool submit(const NavigationRequest& request); + + /// @brief Đồng bộ nav_feedback_ với state hiện tại của lõi. + void refreshFeedback(); + + /** + * @brief Đưa lệnh vận tốc của cycle vừa chạy ra @ref getTwist. + * + * `getTwist()` của contract host là **lệnh đang phát**, không phải vận tốc đo được: host lấy nó + * publish thẳng ra cmd_vel. Nguồn duy nhất hợp lệ là đầu ra của VelocityArbiter. + */ + void publishCommand(); + + /** + * @brief Đẩy trần vận tốc và vận tốc đo được mà host đã đặt xuống controller. + * + * Gọi từ @ref spinOnce, tức **control thread**. Host đặt các giá trị này từ thread của nó + * (OPC-UA, VDA5050, ROS); `ControllerPort` không thread-safe nên chúng phải được cất lại rồi đẩy + * xuống ở đây, không gọi thẳng. + */ + void pushHostInputsToController(); + + /// @brief Ánh xạ state của lõi sang enum trạng thái của contract host. + static robot::move_base_core::State toHostState(NavigationState state); + + ControlLoop loop_; + SensorGateway sensors_; + robot::TFListenerPtr tf_; + + /// Bảo vệ dữ liệu sensor và footprint: host ghi từ thread của nó, control loop đọc. + mutable std::mutex data_mutex_; + + std::vector footprint_; + 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. + std::string robot_base_frame_ = "base_link"; + + /** + * Trần vận tốc host vừa đặt, chờ được đẩy xuống controller ở cycle kế tiếp. + * + * Đây là đường tầng an toàn hạ tốc độ robot (`amr_control.cpp:561, 671-680`), nên bỏ lỡ một lời + * gọi là bỏ lỡ một yêu cầu giảm tốc. Giữ riêng tiến/lùi vì dấu chọn chiều. + */ + robot_geometry_msgs::Vector3 pending_linear_forward_; + robot_geometry_msgs::Vector3 pending_linear_backward_; + robot_geometry_msgs::Vector3 pending_angular_; + bool has_pending_linear_forward_ = false; + bool has_pending_linear_backward_ = false; + bool has_pending_angular_ = false; + + std::string last_reject_reason_; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_NAVIGATION_SERVER_H_ diff --git a/include/move_base2/ports/action_port.h b/include/move_base2/ports/action_port.h new file mode 100644 index 0000000..4e9dc39 --- /dev/null +++ b/include/move_base2/ports/action_port.h @@ -0,0 +1,92 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng ra phía thực thi action của mission (D8). + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_ACTION_PORT_H_ +#define MOVE_BASE2_PORTS_ACTION_PORT_H_ + +#include + +// robot_protocol_msgs/Action.h khai boost::shared_ptr nhưng không tự include nó. Translation unit +// nào chạm Action.h trước một header boost khác sẽ hỏng, nên nạp ở đây — chỗ duy nhất trong gói +// kéo Action.h vào. +#include + +#include + +namespace robot +{ +class NodeHandle; +} + +namespace move_base2 +{ + +/** + * @struct ActionTick + * @brief Kết quả một control cycle của action đang chạy. + * + * Khác RecoveryTick, tick của action KHÔNG có vận tốc lẫn path: theo D8, trong lúc chạy action + * không ai được phát cmd_vel — action cần chuyển động phải được mô hình hoá thành motion profile + * của navigation. Contract này là hàng rào an toàn, không phải thiếu sót. + */ +struct ActionTick +{ + enum class Status + { + kRunning, + kSucceeded, + kFailed + }; + + Status status = Status::kRunning; + + std::string message; ///< Mô tả người-đọc-được, chỉ để log khi state đổi. +}; + +/** + * @class ActionPort + * @brief Cổng ra phía thực thi action. Tick-based: mỗi control cycle một lời gọi @ref update. + * + * Cùng mô hình với RecoveryPort và cùng lý do (D5): handler được tick từ control thread ở + * `controller_frequency`, KHÔNG được block — handler chờ thiết bị (nâng kệ, sạc…) thì tự giữ state + * và trả kRunning, nhờ vậy cancel/pause/emergency luôn được phản hồi trong một cycle. + * + * Contract timeout (3 tầng, tầng 1 là chính): (1) MỖI handler phải tự timeout theo hiểu biết + * 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", chỉ + * handler biết ngưỡng đúng, quá ngưỡng thì trả kFailed; (2) `action_patience` của state machine là + * lưới cuối cho handler treo, mặc định tắt; (3) `mission_timeout` của mission layer đo cả chặng. + * Handler trả kRunning vĩnh viễn mà không có đường thoát riêng là handler viết sai contract. + * + * Phase 4 hiện thực port này bằng ActionRunner: nạp ActionHandler plugin qua boost::dll + + * `library_path`, route theo actionType VDA5050. Ở Phase 1 chỉ có fake cho test. + */ +class ActionPort +{ +public: + virtual ~ActionPort() = default; + + /// @brief Nạp và cấu hình các action handler. Gọi một lần lúc khởi tạo. + virtual bool configure(robot::NodeHandle& nh) = 0; + + /** + * @brief Bắt đầu một action. + * @return false nếu không có handler nào nhận actionType này hoặc handler từ chối khởi động — + * bên gọi coi như action thất bại, không được tick tiếp. + */ + virtual bool start(const robot_protocol_msgs::Action& action) = 0; + + /// @brief Một control cycle. CHỈ được gọi sau khi @ref start trả true. + virtual ActionTick update() = 0; + + /// @brief Yêu cầu dừng an toàn action đang chạy (huỷ mission, emergency). + virtual void cancel() = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_ACTION_PORT_H_ diff --git a/include/move_base2/ports/clock_port.h b/include/move_base2/ports/clock_port.h new file mode 100644 index 0000000..b3dcf92 --- /dev/null +++ b/include/move_base2/ports/clock_port.h @@ -0,0 +1,46 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng thời gian. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_CLOCK_PORT_H_ +#define MOVE_BASE2_PORTS_CLOCK_PORT_H_ + +#include + +namespace move_base2 +{ + +/** + * @class ClockPort + * @brief Nguồn thời gian của runtime. + * + * Mọi ngưỡng thời gian của navigation (planner_patience, controller_patience, oscillation_timeout) + * và mọi phép tích phân theo dt đều đi qua cổng này. Lý do: nếu lấy thẳng robot::Time::now() thì + * không cách nào kiểm được hành vi khi control loop chạy chậm hơn chu kỳ cấu hình — đúng lớp lỗi + * mà dead-reckoning theo chu kỳ cấu hình mắc phải. + */ +class ClockPort +{ +public: + virtual ~ClockPort() = default; + + virtual robot::Time now() const = 0; +}; + +/// @brief Cổng thời gian dùng ở runtime thật. +class SystemClock final : public ClockPort +{ +public: + robot::Time now() const override + { + return robot::Time::now(); + } +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_CLOCK_PORT_H_ diff --git a/include/move_base2/ports/controller_port.h b/include/move_base2/ports/controller_port.h new file mode 100644 index 0000000..0feb436 --- /dev/null +++ b/include/move_base2/ports/controller_port.h @@ -0,0 +1,97 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng ra phía local planner (controller). + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_CONTROLLER_PORT_H_ +#define MOVE_BASE2_PORTS_CONTROLLER_PORT_H_ + +#include +#include + +#include +#include +#include + +namespace move_base2 +{ + +/** + * @class ControllerPort + * @brief Cổng ra phía local planner (thế hệ 1: robot_nav_core::BaseLocalPlanner). + * + * Giữ nguyên bộ hàm của interface được bọc — kể cả việc `isGoalReached()` tách khỏi + * `computeVelocityCommands()`. Thứ tự gọi cũng giữ nguyên: hỏi đã tới đích trước, chỉ khi chưa mới + * tính lệnh. Đổi thứ tự này sẽ đổi hành vi tại đích của mọi local planner đang chạy. + */ +class ControllerPort +{ +public: + virtual ~ControllerPort() = default; + + /** + * @brief Đổi local planner đang dùng theo profile của yêu cầu. + * @return false nếu không nạp được — bên gọi phải từ chối yêu cầu. + */ + 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] + */ + virtual void setTolerance(double xy_m, double yaw_rad) = 0; + + /** + * @brief Nạp plan mới. + * @return false nếu controller từ chối plan (rỗng, sai frame, không bám được). + */ + virtual bool setPlan(const std::vector& plan) = 0; + + /** + * @brief Tính lệnh vận tốc cho cycle này. + * @param[out] cmd [m/s], [rad/s]. CHỈ hợp lệ khi hàm trả true. + * @return false khi không sinh được lệnh hợp lệ ở cycle này. + */ + virtual bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) = 0; + + /// @brief Đã tới đích theo sai số đã đặt hay chưa. + virtual bool isGoalReached() = 0; + + /** + * @brief Vận tốc đo được của robot, dùng làm dữ liệu vào cho lần tính lệnh kế tiếp. + * + * Interface được bọc nhận vận tốc hiện tại như tham số của `computeVelocityCommands`; bản cũ lấy + * nó từ `odometry_.twist.twist`. Truyền **theo giá trị** qua cổng này thay vì cho controller giữ + * con trỏ tới bộ nhớ do host ghi — bản cũ làm thế (`tc_->setOdom(&odometry_)`) và đó là một data + * race không có gì bảo vệ: host ghi từ thread của nó, control thread đọc qua con trỏ. + * + * @param velocity [m/s], [rad/s] trong hệ thân xe. + */ + virtual void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) = 0; + + /** + * @brief Đặt trần vận tốc thẳng. **Dấu chọn chiều**: dương = tiến, âm = lùi. + * + * Đây không phải lệnh jog dù tên nghe như vậy. Host gọi theo cặp `+v` rồi `-v` để đặt trần cho cả + * hai chiều, và giá trị nó truyền xuống mang theo **tốc độ đã bị tầng an toàn hạ xuống** + * (`amr_control.cpp:671-680`). Bỏ qua lời gọi này nghĩa là tầng an toàn yêu cầu giảm tốc mà robot + * vẫn chạy nguyên tốc độ planner. + * + * @return false nếu controller đang dùng không hỗ trợ đặt trần. + */ + virtual bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) = 0; + + /// @brief Đặt trần vận tốc góc. Quy ước dấu như @ref setTwistLinear. + virtual bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) = 0; + + /// @brief Tên controller đang hoạt động; chuỗi rỗng nếu chưa nạp được. + virtual std::string activeController() const = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_CONTROLLER_PORT_H_ diff --git a/include/move_base2/ports/mission_port.h b/include/move_base2/ports/mission_port.h new file mode 100644 index 0000000..20ba838 --- /dev/null +++ b/include/move_base2/ports/mission_port.h @@ -0,0 +1,70 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng ra phía mission layer. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_MISSION_PORT_H_ +#define MOVE_BASE2_PORTS_MISSION_PORT_H_ + +#include +#include + +#include + +namespace move_base2 +{ + +/// @brief Kết quả của một chặng navigation, theo ngôn ngữ mà mission layer cần để chạy tiếp hàng đợi. +enum class NavigationOutcome +{ + kSucceeded, + kFailed, + kCancelled, + kPreempted +}; + +/// @brief Tên outcome dạng chuỗi. +const char* toString(NavigationOutcome outcome); + +/** + * @class MissionPort + * @brief Cổng ra phía mission. move_base2 KHÔNG biết mission framework nào đang chạy phía sau. + * + * Luồng một chiều, không polling: mission layer đẩy chặng xuống qua callback đã đăng ký; move_base2 + * báo ngược kết quả qua @ref reportOutcome. Bản thế hệ 1 không có khái niệm này nên phía mission + * phải poll trường trạng thái của navigation — cách đó dễ trượt sự kiện hoặc đếm hai lần khi state + * đổi nhanh hơn nhịp poll. + * + * @invariant @ref reportOutcome chỉ được gọi ĐÚNG MỘT LẦN cho mỗi + * NavigationRequest::mission_sequence_id khác 0. + */ +class MissionPort +{ +public: + using RequestCallback = std::function; + + virtual ~MissionPort() = default; + + /// @brief Đăng ký callback mà mission layer gọi mỗi khi có chặng mới cần chạy. + virtual void setRequestCallback(RequestCallback callback) = 0; + + /** + * @brief Báo kết quả chặng vừa chạy. + * @param mission_sequence_id Echo lại từ NavigationRequest. + * @param outcome Kết quả chặng. + */ + virtual void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) = 0; + + /// @brief Có mission đang chạy hay không. Chỉ dùng để publish feedback, KHÔNG dùng để điều khiển. + virtual bool hasActiveMission() const = 0; + + virtual void start() = 0; + virtual void stop() = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_MISSION_PORT_H_ diff --git a/include/move_base2/ports/planner_port.h b/include/move_base2/ports/planner_port.h new file mode 100644 index 0000000..c96d265 --- /dev/null +++ b/include/move_base2/ports/planner_port.h @@ -0,0 +1,109 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng ra phía global planner. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_PLANNER_PORT_H_ +#define MOVE_BASE2_PORTS_PLANNER_PORT_H_ + +#include +#include +#include + +#include +#include + +namespace move_base2 +{ + +/** + * @struct PlanResult + * @brief Kết quả một lượt lập plan. + */ +struct PlanResult +{ + /** + * @brief Nhãn của yêu cầu sinh ra kết quả này, chép từ @ref PlannerPort::startPlan. + * + * Bên gọi **phải** so nhãn này với yêu cầu đang chạy và vứt kết quả không khớp. Lập plan mất hàng + * trăm ms; trong khoảng đó goal có thể đã đổi, và bám theo một plan tới goal cũ nghĩa là robot đi + * tới chỗ không ai yêu cầu. + */ + std::uint64_t tag = 0; + + /// Lượt lập plan có ra được plan không rỗng hay không. + bool succeeded = false; + + /// Plan trong global frame. Chỉ có nghĩa khi @ref succeeded. + std::vector plan; +}; + +/** + * @class PlannerPort + * @brief Cổng ra phía global planner (thế hệ 1: robot_nav_core::BaseGlobalPlanner). + * + * Contract **bất đồng bộ**: `startPlan` trả về ngay, kết quả lấy sau bằng `pollPlan`. Lý do là ràng + * buộc thời gian thật chứ không phải sở thích kiến trúc — global planner nặng (SBPL lattice) mất + * hàng trăm ms, trong khi control loop chạy 20 Hz và là thread **duy nhất** được phát `cmd_vel`. + * Lập plan đồng bộ nghĩa là mỗi lần lập lại plan là ngần ấy thời gian robot chạy bằng lệnh cũ mà + * không ai giám sát. + * + * Hai overload `makePlan` của interface gốc được gộp lại: "có Order hay không" chỉ là một nhánh nhỏ + * bên trong hiện thực, không đáng nhân đôi contract. + */ +class PlannerPort +{ +public: + virtual ~PlannerPort() = default; + + /** + * @brief Đổi global planner đang dùng. + * @param planner_name Tên alias plugin, khớp key trong config. + * @return false nếu không nạp được — bên gọi phải từ chối yêu cầu, không đi tiếp với planner cũ. + */ + virtual bool swapPlanner(const std::string& planner_name) = 0; + + /** + * @brief Khởi động một lượt lập plan. **Không chặn.** + * + * @param order Order gốc nếu yêu cầu đến từ giao thức fleet; nullptr nếu là goal trực tiếp. + * Hiện thực **phải sao chép** nội dung — con trỏ chỉ hợp lệ trong lời gọi này, còn + * lượt lập plan sống lâu hơn thế. + * @param tag Nhãn phân biệt yêu cầu, trả lại nguyên vẹn trong @ref PlanResult::tag. + * + * @return false nếu không khởi động được: chưa có planner, hoặc đang có một lượt chạy dở. + */ + virtual bool startPlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + const robot_protocol_msgs::Order* order, std::uint64_t tag) = 0; + + /// @brief Có lượt lập plan nào đang chạy không. + virtual bool isPlanning() const = 0; + + /** + * @brief Lấy kết quả nếu đã có. Không chặn. + * + * @param[out] result Chỉ được ghi khi hàm trả true. Hiện thực nên **hoán vị** vector plan với + * `result.plan` thay vì copy, để bộ nhớ được tái sử dụng qua các lượt. + * @return true khi có kết quả — kể cả kết quả thất bại. false nghĩa là chưa xong hoặc không có gì. + */ + virtual bool pollPlan(PlanResult& result) = 0; + + /** + * @brief Bỏ lượt đang chạy. + * + * @note Không cắt ngang được plugin đang tính: nó là hộp đen nạp lúc chạy, không có đường ngắt. + * Huỷ ở đây nghĩa là **vứt kết quả khi nó về**, không phải dừng phép tính. + */ + virtual void cancelPlan() = 0; + + /// @brief Tên planner đang hoạt động; chuỗi rỗng nếu chưa nạp được planner nào. + virtual std::string activePlanner() const = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_PLANNER_PORT_H_ diff --git a/include/move_base2/ports/pose_port.h b/include/move_base2/ports/pose_port.h new file mode 100644 index 0000000..d46318c --- /dev/null +++ b/include/move_base2/ports/pose_port.h @@ -0,0 +1,38 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng lấy pose robot. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_POSE_PORT_H_ +#define MOVE_BASE2_PORTS_POSE_PORT_H_ + +#include + +namespace move_base2 +{ + +/** + * @class PosePort + * @brief Nguồn pose robot trong global frame. + * + * Ở runtime thật, hiện thực bọc Costmap2DROBOT::getRobotPose (đã bao gồm tra TF và kiểm + * transform_tolerance). Trong test, hiện thực là fake bơm pose theo kịch bản. + * + * @invariant Trả false nghĩa là "không biết robot đang ở đâu" — TF thiếu, TF quá hạn, hoặc frame + * chưa sẵn sàng. Bên gọi PHẢI dừng an toàn, không được dùng pose cũ để đi tiếp. + * @p pose không được ghi khi hàm trả false. + */ +class PosePort +{ +public: + virtual ~PosePort() = default; + + virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_POSE_PORT_H_ diff --git a/include/move_base2/ports/recovery_port.h b/include/move_base2/ports/recovery_port.h new file mode 100644 index 0000000..6e32744 --- /dev/null +++ b/include/move_base2/ports/recovery_port.h @@ -0,0 +1,135 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cổng ra phía recovery. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_PORTS_RECOVERY_PORT_H_ +#define MOVE_BASE2_PORTS_RECOVERY_PORT_H_ + +#include +#include +#include + +#include +#include + +namespace robot +{ +class NodeHandle; +} + +namespace move_base2 +{ + +/// @brief Lý do vào recovery. Behavior có thể dùng để chọn chiến lược khác nhau cho cùng một plugin. +enum class RecoveryTrigger +{ + kPlanningFailed, ///< Không lập được plan trong thời gian cho phép. + kControllingFailed, ///< Không sinh được lệnh vận tốc hợp lệ trong thời gian cho phép. + kOscillation ///< Robot quẩn tại chỗ quá lâu. +}; + +const char* toString(RecoveryTrigger trigger); + +/** + * @enum RecoveryOutputKind + * @brief Behavior đó có lái robot hay không. + * + * Lõi cần biết điều này **trước** khi behavior chạy tick đầu tiên: nếu behavior không phát vận tốc + * (đợi, xoá costmap) thì nguồn vận tốc của cycle phải là `kNone`, không phải `kRecovery`. Coi mọi + * behavior là nguồn vận tốc khiến `VelocityArbiter` đổi nguồn hai lần cho mỗi lượt one-shot, mỗi + * lần chèn một cycle zero — vài chục ms cmd_vel = 0 không vì lý do gì. + * + * Khai riêng ở đây thay vì dùng `recovery_core::RecoveryOutputType`: lõi không được include + * recovery framework (D3). `RecoveryRunner` map 1:1 hai enum bằng `switch`. + */ +enum class RecoveryOutputKind +{ + kNone, ///< Không phát output (đợi, xoá costmap). + kVelocity, ///< Phát Twist mỗi cycle (rotate, back up). + kPath ///< Sinh đường đi mới. +}; + +const char* toString(RecoveryOutputKind kind); + +/** + * @struct RecoveryTick + * @brief Kết quả một control cycle của recovery, đã chuẩn hoá về ngôn ngữ của move_base2. + * + * Chuẩn hoá ở đây thay vì dùng thẳng kiểu của recovery framework là có chủ đích: lõi không được + * include recovery framework, nếu không chiều phụ thuộc sẽ khoá cứng move_base2 vào một hiện thực. + * + * Bất biến đọc: chỉ đọc @ref cmd khi @ref has_velocity, chỉ đọc @ref path khi @ref has_path. Một + * tick không bao giờ vừa có vận tốc vừa có path. + */ +struct RecoveryTick +{ + enum class Status + { + kRunning, + kSucceeded, + kFailed + }; + + Status status = Status::kRunning; + + bool has_velocity = false; ///< true -> @ref cmd hợp lệ. + robot_geometry_msgs::Twist cmd; ///< [m/s], [rad/s]. Dấu âm của linear.x nghĩa là lùi. + + bool has_path = false; ///< true -> @ref path hợp lệ (họ recovery sinh lại đường đi). + std::vector path; + + std::string message; ///< Mô tả người-đọc-được, chỉ để log khi state đổi. +}; + +/** + * @class RecoveryPort + * @brief Cổng ra phía recovery. Tick-based: mỗi control cycle một lời gọi @ref update. + * + * Đây là khác biệt kiến trúc so với thế hệ 1, không phải đổi tên hàm. Recovery thế hệ 1 chạy + * blocking bên trong một lời gọi và không trả gì; recovery thế hệ 2 trả kết quả từng cycle và có + * thể phát vận tốc. Hệ quả bắt buộc: recovery phải được tick từ đúng thread đang sở hữu cmd_vel, + * không được có thread riêng — hai thread cùng phát vận tốc là hai bộ điều khiển tranh nhau. + */ +class RecoveryPort +{ +public: + virtual ~RecoveryPort() = default; + + /// @brief Nạp và cấu hình danh sách behavior. Gọi một lần lúc khởi tạo. + virtual bool configure(robot::NodeHandle& nh) = 0; + + /// @brief Số behavior đã nạp được. 0 nghĩa là không có đường phục hồi nào. + virtual std::size_t behaviorCount() const = 0; + + /** + * @brief Họ output của behavior thứ @p index — behavior đó có lái robot hay không. + * + * Được hỏi **trước** khi behavior chạy, kể cả ở cycle khởi động nó. Index sai phải trả + * @ref RecoveryOutputKind::kNone (giả định an toàn: không cấp quyền phát vận tốc cho thứ không + * biết là gì). + */ + virtual RecoveryOutputKind outputKind(std::size_t index) const = 0; + + /** + * @brief Bắt đầu behavior thứ @p index. + * @return false nếu index sai hoặc behavior từ chối khởi động (ví dụ đã va chạm ngay tại chỗ). + */ + virtual bool start(std::size_t index, RecoveryTrigger trigger) = 0; + + /// @brief Một control cycle. CHỈ được gọi sau khi @ref start trả true. + virtual RecoveryTick update() = 0; + + /// @brief Yêu cầu dừng. Tick kế tiếp phải trả kết quả dừng an toàn. + virtual void cancel() = 0; + + /// @brief Tên behavior thứ @p index; chuỗi rỗng nếu index sai. + virtual std::string behaviorName(std::size_t index) const = 0; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_PORTS_RECOVERY_PORT_H_ diff --git a/include/move_base2/runners/action_handler.h b/include/move_base2/runners/action_handler.h new file mode 100644 index 0000000..2c019e6 --- /dev/null +++ b/include/move_base2/runners/action_handler.h @@ -0,0 +1,79 @@ +/********************************************************************* + * + * 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 new file mode 100644 index 0000000..6f9318c --- /dev/null +++ b/include/move_base2/runners/action_runner.h @@ -0,0 +1,112 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực ActionPort bằng các ActionHandler plugin. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_ +#define MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_ + +#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. + * + * Cấu hình mong đợi: + * + * @code{.yaml} + * actions: + * handlers: + * - {name: noop, type: NoopActionHandler} + * noop: + * action_types: [wait, pick, drop] + * duration: 0.0 # [s] + * + * NoopActionHandler: + * library_path: libmove_base2_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. + * + * @note Không thread-safe. Chỉ control thread được gọi. + */ +class ActionRunner final : public ActionPort +{ +public: + ActionRunner() = default; + ~ActionRunner() override; + + ActionRunner(const ActionRunner&) = delete; + ActionRunner& operator=(const ActionRunner&) = delete; + + /// @brief Đồng hồ runtime. Bắt buộc đặt trước @ref configure. + void setClock(ClockPort* clock); + + /// @brief Namespace YAML chứa `/handlers`. Mặc định "actions". + void setNamespace(const std::string& ns); + + /** + * @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 configure(robot::NodeHandle& nh) override; + bool start(const robot_protocol_msgs::Action& action) override; + ActionTick update() override; + void cancel() override; + + /// @brief Số handler đã nạp. + std::size_t handlerCount() const + { + return handlers_.size(); + } + + /// @brief Các `actionType` đã có handler nhận. Dùng cho log và test. + std::vector supportedActionTypes() const; + + /// @brief Handler nhận @p action_type, hoặc nullptr. + ActionHandler* find(const std::string& action_type) const; + +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); + + ClockPort* clock_ = nullptr; ///< non-owning + std::string namespace_ = "actions"; + bool configured_ = false; + + std::vector handlers_; + std::map by_type_; ///< non-owning, trỏ vào handlers_ + + ActionHandler* active_ = nullptr; ///< non-owning + 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 + +#endif // MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_ diff --git a/include/move_base2/runners/controller_runner.h b/include/move_base2/runners/controller_runner.h new file mode 100644 index 0000000..3000920 --- /dev/null +++ b/include/move_base2/runners/controller_runner.h @@ -0,0 +1,157 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực ControllerPort bằng plugin robot_nav_core::BaseLocalPlanner. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_ +#define MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_ + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace robot_costmap_2d +{ +class Costmap2DROBOT; +} +namespace tf3 +{ +class BufferCore; +} + +namespace move_base2 +{ + +/** + * @class ControllerRunner + * @brief Nạp và chạy local planner thế hệ 1 (`robot_nav_core::BaseLocalPlanner`) qua Boost.DLL. + * + * Đây là chỗ duy nhất trong gói biết tới `robot_nav_core::BaseLocalPlanner`. Lõi quyết định chỉ thấy + * @ref ControllerPort. + * + * ## Chạy đồng bộ, có chủ đích + * + * Khác @ref PlannerRunner, controller **không** cần thread riêng: nó phải trả lệnh trong đúng cycle + * hiện tại. Một local planner mất hơn một chu kỳ điều khiển là lỗi cấu hình của chính nó, không phải + * thứ kiến trúc ở đây che đi được — che đi sẽ thành robot chạy bằng lệnh cũ mà không ai biết. + * + * ## Sai số tại đích đi qua param server + * + * `BaseLocalPlanner` **không có** hàm đặt sai số. Bản cũ đặt qua `NodeHandle::setParam` và để + * planner tự đọc lại (`move_base.cpp:1897-1907`). Kênh đó gián tiếp và mong manh — planner nào đọc + * param một lần lúc `initialize` sẽ không bao giờ thấy giá trị mới — nhưng đổi nó là đổi interface + * gen-1, ảnh hưởng mọi planner đang chạy. Giữ nguyên, và ghi lại ở đây để không ai tưởng nó chắc + * chắn có tác dụng. + * + * ## Trần vận tốc (`setTwistLinear`) + * + * Tên nghe như lệnh jog nhưng thực chất là đặt trần, dấu chọn chiều. Đường này mang **tốc độ đã bị + * tầng an toàn hạ xuống**; bỏ qua nó là bỏ qua yêu cầu giảm tốc của tầng an toàn. + * + * @note **Không thread-safe — mọi hàm chỉ được gọi từ control thread.** + * Host đặt trần vận tốc và bơm odometry từ thread của nó (OPC-UA, VDA5050, ROS), nên + * `NavigationServer` phải cất các giá trị đó lại và đẩy xuống đây trong `spinOnce()`. Gọi + * thẳng từ thread host sẽ chạm `active_` và chạm plugin song song với lúc control thread đang + * tính lệnh. + */ +class ControllerRunner : public ControllerPort +{ +public: + ControllerRunner(); + ~ControllerRunner() override; + + ControllerRunner(const ControllerRunner&) = delete; + ControllerRunner& operator=(const ControllerRunner&) = delete; + + /** + * @brief Nạp cấu hình và controller khởi đầu. + * + * @param nh NodeHandle để tra `library_path` và để đặt sai số tại đích. Được + * **sao chép** vì cả hai việc đó xảy ra lúc chạy. + * @param tf Buffer TF truyền cho `BaseLocalPlanner::initialize`. **Non-owning**. + * @param costmap Costmap local. **Non-owning**, bắt buộc khác null. + * @param initial_controller Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner. + * @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false. + */ + bool configure(const robot::NodeHandle& nh, tf3::BufferCore* tf, + robot_costmap_2d::Costmap2DROBOT* costmap, + const std::string& initial_controller, std::string& error); + + bool configured() const + { + return configured_; + } + + // ================================================================================================ + // ControllerPort + // ================================================================================================ + + bool swapPlanner(const std::string& planner_name) override; + void setTolerance(double xy_m, double yaw_rad) override; + bool setPlan(const std::vector& plan) override; + bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override; + bool isGoalReached() override; + void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override; + bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override; + bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override; + std::string activeController() const override; + + /// @brief Số plugin đã nạp và còn giữ trong cache. + std::size_t loadedCount() const + { + return controllers_.size(); + } + +private: + /// @brief Một plugin đã nạp: factory phải sống cùng instance — vứt factory là để `.so` unload. + struct Loaded + { + std::function factory; + robot_nav_core::BaseLocalPlanner::Ptr instance; + }; + + /// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do). + robot_nav_core::BaseLocalPlanner* acquire(const std::string& name); + + /// @brief Áp lại trần vận tốc và sai số đã lưu lên controller vừa đổi sang. + void applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller); + + robot::NodeHandle nh_; + tf3::BufferCore* tf_ = nullptr; + robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; + bool configured_ = false; + + std::map controllers_; + std::string active_name_; + robot_nav_core::BaseLocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_. + + /** + * Trần vận tốc và vận tốc đo được gần nhất. + * + * Giữ lại bản sao vì hai lý do: host đặt trần **trước** khi controller được nạp (thứ tự khởi tạo + * không do move_base2 quyết), và @ref swapPlanner đổi sang một instance chưa biết gì về các trần + * đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép. + */ + robot_geometry_msgs::Vector3 limit_linear_forward_; + robot_geometry_msgs::Vector3 limit_linear_backward_; + robot_geometry_msgs::Vector3 limit_angular_; + bool has_limit_linear_forward_ = false; + bool has_limit_linear_backward_ = false; + bool has_limit_angular_ = false; + + robot_geometry_msgs::Twist measured_velocity_; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_ diff --git a/include/move_base2/runners/planner_runner.h b/include/move_base2/runners/planner_runner.h new file mode 100644 index 0000000..e1f40b5 --- /dev/null +++ b/include/move_base2/runners/planner_runner.h @@ -0,0 +1,176 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực PlannerPort bằng plugin robot_nav_core::BaseGlobalPlanner, chạy trên + * thread riêng. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_ +#define MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace robot_costmap_2d +{ +class Costmap2DROBOT; +} + +namespace move_base2 +{ + +/** + * @class PlannerRunner + * @brief Nạp và chạy global planner thế hệ 1 (`robot_nav_core::BaseGlobalPlanner`) qua Boost.DLL, + * trên một thread riêng. + * + * Đây là chỗ duy nhất trong gói biết tới `robot_nav_core`. Lõi quyết định chỉ thấy @ref PlannerPort. + * + * ## Vì sao có thread riêng + * + * Global planner nặng mất hàng trăm ms; control loop chạy 20 Hz và là thread duy nhất phát + * `cmd_vel`. Lập plan tại chỗ nghĩa là mỗi lần lập lại plan là ngần ấy thời gian robot chạy bằng + * lệnh cũ không ai giám sát. + * + * ## Bàn giao plan bằng hoán vị, không copy + * + * Một plan toàn cục có thể vài nghìn pose. Copy nó mỗi lượt là cấp phát lớn trên đường nóng. Ở đây + * có **ba** vector luân chuyển bằng `swap`, đúng mô hình triple buffer của bản cũ nhưng bằng giá + * trị thay vì con trỏ thô: + * + * ``` + * planning_ thread planner ghi vào + * handoff_ hộp thư, đổi dưới mutex + * (của bên gọi) pollPlan hoán vị với handoff_ -> vector cũ của bên gọi quay lại làm hộp thư + * ``` + * + * Mutex chỉ bị giữ trong lúc đổi vector, không bao giờ trong lúc plugin đang tính. + * + * ## Nhãn yêu cầu (tag) + * + * Lượt lập plan sống lâu hơn cái goal sinh ra nó. Mỗi lượt mang một nhãn do bên gọi cấp; bên gọi so + * nhãn và vứt kết quả không khớp. Không có nó thì một plan tới goal đã bị huỷ vẫn được bám theo. + * + * ## Giữ instance sống bằng cách nào + * + * `boost::dll::import_alias` trả về factory **giữ tham chiếu tới thư viện đã nạp**. Vứt factory đi + * trong khi instance nó tạo ra còn sống là để `.so` bị unload dưới chân object. Mỗi entry vì thế + * giữ **cả hai**. Cache cũng **không bao giờ xoá** entry — nhờ đó con trỏ planner mà thread đang + * dùng vẫn hợp lệ kể cả khi @ref swapPlanner đổi sang planner khác giữa chừng. + * + * @note Các hàm public gọi từ control thread. Thread nội bộ chỉ chạm dữ liệu dưới mutex. + * @warning Destructor **chờ** lượt đang chạy kết thúc: plugin là hộp đen, không có đường cắt ngang. + */ +class PlannerRunner : public PlannerPort +{ +public: + PlannerRunner(); + ~PlannerRunner() override; + + PlannerRunner(const PlannerRunner&) = delete; + PlannerRunner& operator=(const PlannerRunner&) = delete; + + /** + * @brief Nạp cấu hình, planner khởi đầu, và khởi động thread. + * + * @param nh NodeHandle để tra `library_path`. Được **sao chép** vì @ref swapPlanner + * cần tra lại lúc chạy. + * @param costmap Costmap global truyền cho `BaseGlobalPlanner::initialize`. **Non-owning**, + * bắt buộc khác null, phải sống lâu hơn object này. + * @param initial_planner Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner. + * @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false. + */ + bool configure(const robot::NodeHandle& nh, robot_costmap_2d::Costmap2DROBOT* costmap, + const std::string& initial_planner, std::string& error); + + bool configured() const + { + return configured_; + } + + // ================================================================================================ + // PlannerPort + // ================================================================================================ + + bool swapPlanner(const std::string& planner_name) override; + + 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; + + bool isPlanning() const override; + + bool pollPlan(PlanResult& result) override; + + void cancelPlan() override; + + std::string activePlanner() const override; + + /// @brief Số plugin đã nạp và còn giữ trong cache — để kiểm việc dùng lại, không phải để log. + std::size_t loadedCount() const + { + return planners_.size(); + } + +private: + /// @brief Một plugin đã nạp: factory phải sống cùng instance, xem doc của lớp. + struct Loaded + { + std::function factory; + robot_nav_core::BaseGlobalPlanner::Ptr instance; + }; + + /// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do). + robot_nav_core::BaseGlobalPlanner* acquire(const std::string& name); + + /// @brief Thân thread: ngủ tới khi có yêu cầu, chạy plugin, đặt kết quả vào hộp thư. + void threadBody(); + + // --- Chỉ control thread chạm ------------------------------------------------------------------ + robot::NodeHandle nh_; + robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; + bool configured_ = false; + std::map planners_; + std::string active_name_; + + // --- Chia sẻ giữa hai thread, bảo vệ bởi mutex_ ------------------------------------------------ + mutable std::mutex mutex_; + std::condition_variable cv_; + std::thread thread_; + + bool shutdown_ = false; + bool pending_ = false; ///< Có yêu cầu chờ thread nhận. + bool running_ = false; ///< Thread đang chạy plugin. + bool discard_ = false; ///< Lượt đang chạy đã bị huỷ — vứt kết quả khi nó về. + bool has_result_ = false; + + robot_nav_core::BaseGlobalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref planners_. + robot_geometry_msgs::PoseStamped request_start_; + robot_geometry_msgs::PoseStamped request_goal_; + std::shared_ptr request_order_; + std::uint64_t request_tag_ = 0; + + std::vector planning_; ///< Thread ghi vào. + std::vector handoff_; ///< Hộp thư. + std::uint64_t result_tag_ = 0; + bool result_ok_ = false; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_ diff --git a/include/move_base2/runners/recovery_runner.h b/include/move_base2/runners/recovery_runner.h new file mode 100644 index 0000000..269db09 --- /dev/null +++ b/include/move_base2/runners/recovery_runner.h @@ -0,0 +1,164 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực RecoveryPort bằng recovery_core. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_ +#define MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace robot_costmap_2d { class Costmap2DROBOT; } + +namespace move_base2 +{ + +/** + * @class RecoveryRunner + * @brief Nối @ref RecoveryPort của lõi với framework `recovery_core`. + * + * Đây là **file duy nhất** trong `move_base2` include `recovery_core`. Nhờ vậy hai bên nằm trong + * cùng một translation unit và compiler kiểm được toàn bộ contract, dù plugin vẫn nạp qua + * `boost::dll` lúc chạy. + * + * Nó cũng là chỗ dịch **ba cặp khái niệm song song** giữa hai gói — dịch ở đúng một nơi: + * + * | `move_base2` | `recovery_core` | + * |---|---| + * | `PosePort` | `PoseProvider` (adapter nội bộ bên dưới) | + * | `ClockPort` | tham số `robot::Time` của `update(now)` | + * | `RecoveryTrigger` | `RecoveryTrigger` (`switch` 1:1) | + * + * Trùng khái niệm là **cố ý**: chiều phụ thuộc một chiều (D3) cấm `recovery_core` biết tới + * `move_base2`. Đừng sinh phiên bản thứ ba của cùng khái niệm ở nơi khác. + * + * @note Không thread-safe. Chỉ control thread được gọi — thread duy nhất phát cmd_vel. + */ +class RecoveryRunner final : public RecoveryPort +{ +public: + /// @brief Nguồn plan hiện hành, cấp cho behavior họ path. Trả false nếu chưa có plan. + using PlanSource = std::function&)>; + + /** + * @struct Deps + * @brief Các cổng runtime mà runner cần. Tất cả **non-owning**. + * + * Con trỏ costmap có thể bị thay giữa hai lượt — dùng @ref setCostmaps để cập nhật thay vì để + * runner giữ một bản cache từ lúc configure. + */ + struct Deps + { + ClockPort* clock = nullptr; + PosePort* pose = nullptr; + robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr; + robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr; + }; + + RecoveryRunner() = default; + + /// @brief Nạp cổng. Gọi trước @ref configure. + void setDeps(const Deps& deps); + + /// @brief Cập nhật con trỏ costmap khi chúng bị thay. Non-owning. + void setCostmaps(robot_costmap_2d::Costmap2DROBOT* local, + robot_costmap_2d::Costmap2DROBOT* global); + + /// @brief Namespace YAML chứa `/behaviors`. Mặc định "recovery". + void setNamespace(const std::string& ns); + + /// @brief Nguồn plan cho behavior họ path. Không đặt = không có plan. + void setPlanSource(PlanSource source); + + /** + * @copydoc RecoveryPort::configure + * + * Nạp danh sách behavior qua `recovery_core::RecoveryRegistry`. Behavior nào `configure()` hỏng + * thì bị bỏ và log đích danh — trả `false`, nhưng các behavior còn lại **vẫn** dùng được: một + * đường phục hồi hỏng không nên xoá sạch các đường còn lại. + */ + bool configure(robot::NodeHandle& nh) override; + + std::size_t behaviorCount() const override; + RecoveryOutputKind outputKind(std::size_t index) const override; + bool start(std::size_t index, RecoveryTrigger trigger) override; + RecoveryTick update() override; + void cancel() override; + std::string behaviorName(std::size_t index) const override; + +private: + /// Chuyển `PosePort` của lõi thành cổng pose của recovery_core. + class PoseBridge final : public recovery_core::PoseProvider + { + public: + explicit PoseBridge(const PosePort* port = nullptr) : port_(port) + { + } + + void setPort(const PosePort* port) + { + port_ = port; + } + + bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override + { + return port_ != nullptr && port_->getRobotPose(pose); + } + + private: + const PosePort* port_ = nullptr; ///< non-owning + }; + + /// Chuyển callback plan của runtime thành cổng plan của recovery_core. + class PlanBridge final : public recovery_core::PlanProvider + { + public: + void setSource(PlanSource source) + { + source_ = std::move(source); + } + + bool getGlobalPlan(std::vector& out) const override + { + return source_ ? source_(out) : false; + } + + private: + PlanSource source_; + }; + + /// Trỏ lại context vào con trỏ costmap hiện hành trước mỗi lượt dùng. + void refreshContext(); + + /// Dịch kết quả của recovery_core sang ngôn ngữ của lõi. + RecoveryTick toTick(const recovery_core::RecoveryResult& result) const; + + Deps deps_; + std::string namespace_ = "recovery"; + + recovery_core::RecoveryRegistry registry_; + recovery_core::RecoveryContext ctx_; + recovery_core::CostmapCollisionChecker collision_; + PoseBridge pose_bridge_; + PlanBridge plan_bridge_; + + bool configured_ = false; + recovery_core::RecoveryBehavior* active_ = nullptr; ///< non-owning, thuộc registry_ +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..6867238 --- /dev/null +++ b/package.xml @@ -0,0 +1,84 @@ + + move_base2 + 0.1.0 + + Navigation runtime thế hệ 2: nhận yêu cầu di chuyển, điều phối global planner / local planner / + recovery behavior qua một state machine tường minh, và phát lệnh vận tốc từ đúng một nguồn tại + mỗi thời điểm. + + Gói hiện thực contract host robot::move_base_core::BaseNavigation và được nạp bằng Boost.DLL + (alias MoveBase2) như mọi plugin khác của workspace. Phần lõi quyết định (state machine, bộ + trọng tài vận tốc) là logic thuần, không I/O, nên kiểm được bằng bảng chuyển trạng thái thay vì + phải chạy robot. + + Mọi phụ thuộc ra ngoài đều đi qua port: mission, recovery, global planner, local planner, pose, + clock. Nhờ vậy lõi không biết mission framework hay recovery framework nào đang được dùng, và + chiều phụ thuộc luôn một chiều. + + DuongTD + DuongTD + BSD + + catkin + + move_base_core + move_base_core + + robot_nav_core + robot_nav_core + + robot_costmap_2d + robot_costmap_2d + + robot_cpp + robot_cpp + + robot_time + robot_time + + robot_geometry_msgs + robot_geometry_msgs + + robot_std_msgs + robot_std_msgs + + robot_nav_msgs + robot_nav_msgs + + robot_nav_2d_msgs + robot_nav_2d_msgs + + robot_nav_2d_utils + robot_nav_2d_utils + + robot_sensor_msgs + robot_sensor_msgs + + robot_map_msgs + robot_map_msgs + + robot_protocol_msgs + robot_protocol_msgs + + robot_xmlrpcpp + robot_xmlrpcpp + + tf3 + tf3 + + yaml-cpp + yaml-cpp + + + nav_test_harness + + recovery_core + recovery_core + + laser_filter + laser_filter + + mission_adapters + mission_adapters + + diff --git a/plugins/noop_action_handler.cpp b/plugins/noop_action_handler.cpp new file mode 100644 index 0000000..3e5f7a5 --- /dev/null +++ b/plugins/noop_action_handler.cpp @@ -0,0 +1,171 @@ +/********************************************************************* + * + * 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 new file mode 100644 index 0000000..5d63dfd --- /dev/null +++ b/src/bridges/mission_adapter_bridge.cpp @@ -0,0 +1,239 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt MissionAdapterBridge. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include +#include + +namespace move_base2 +{ + +MissionAdapterBridge::MissionAdapterBridge() = default; +MissionAdapterBridge::~MissionAdapterBridge() = default; + +void MissionAdapterBridge::attach(mission_adapters::MissionManager* manager) +{ + std::lock_guard lock(mutex_); + manager_ = manager; +} + +void MissionAdapterBridge::setCancelCallback(CancelCallback callback) +{ + std::lock_guard lock(mutex_); + cancel_callback_ = std::move(callback); +} + +void MissionAdapterBridge::setRequestCallback(RequestCallback callback) +{ + std::lock_guard lock(mutex_); + request_callback_ = std::move(callback); +} + +// ================================================================================================ +// Chuyển đổi +// ================================================================================================ + +NavigationRequest MissionAdapterBridge::toRequest(const mission_adapters::Mission& mission) +{ + NavigationRequest request; + + request.mission_sequence_id = mission.id; + 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()); + for (const auto& action : mission.actions) + { + 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; + + return request; +} + +// ================================================================================================ +// NavigationClient — gọi từ thread của MissionExecutor +// ================================================================================================ + +bool MissionAdapterBridge::dispatch(const std::shared_ptr& mission) +{ + if (!mission) + { + robot::log_error("[move_base2] MissionAdapterBridge: dispatch(nullptr).\n"); + return false; + } + + std::lock_guard lock(mutex_); + + if (!running_) + { + // Chưa start hoặc đã stop. Từ chối thay vì cất lại: mission layer phải biết chặng của nó không + // được nhận, chứ không phải chờ một kết quả sẽ không bao giờ tới. + robot::log_warning("[move_base2] MissionAdapterBridge: từ chối mission %llu — bridge chưa " + "start.\n", static_cast(mission->id)); + return false; + } + + if (pending_) + { + // Không nên xảy ra: MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Đếm lại thay + // vì im lặng — một chặng biến mất trong khi fleet master vẫn chờ nó là lỗi rất khó truy. + ++dropped_requests_; + robot::log_warning("[move_base2] MissionAdapterBridge: mission %llu đè mission %llu chưa kịp " + "đẩy xuống.\n", static_cast(mission->id), + static_cast(pending_->id)); + } + + // Chỉ cất lại. Chuyển đổi và đẩy xuống navigation xảy ra ở pumpPendingRequest(), trên control + // thread — xem doc của lớp. + pending_ = mission; + return true; +} + +void MissionAdapterBridge::cancelActive(mission_adapters::MissionId /*id*/) +{ + CancelCallback callback; + { + std::lock_guard lock(mutex_); + + // Chặng đang chờ mà chưa kịp xuống navigation thì huỷ ngay tại đây: đẩy nó xuống rồi mới huỷ là + // cho robot nhúc nhích một cycle vì một chặng đã bị thu hồi. + pending_.reset(); + callback = cancel_callback_; + } + + // Gọi NGOÀI lock: callback đi vào control loop, không được chạy dưới mutex của bridge. + if (callback) + { + callback(); + } +} + +// ================================================================================================ +// Biên thread — chỉ control thread gọi +// ================================================================================================ + +bool MissionAdapterBridge::pumpPendingRequest() +{ + std::shared_ptr mission; + RequestCallback callback; + + { + std::lock_guard lock(mutex_); + if (!pending_ || !request_callback_) + { + return false; + } + mission = pending_; + pending_.reset(); + callback = request_callback_; + } + + // Ngoài lock: callback đi thẳng vào ControlLoop::submit. + callback(toRequest(*mission)); + return true; +} + +// ================================================================================================ +// MissionPort — gọi từ control thread +// ================================================================================================ + +void MissionAdapterBridge::reportOutcome(std::uint64_t mission_sequence_id, + NavigationOutcome outcome) +{ + if (mission_sequence_id == mission_adapters::kInvalidMissionId) + { + // Goal trực tiếp từ contract host, không thuộc mission nào. Không có gì để báo. + return; + } + + mission_adapters::MissionManager* manager = nullptr; + { + std::lock_guard lock(mutex_); + manager = manager_; + } + + if (manager == nullptr) + { + return; + } + + // Chỉ kSucceeded mới là "chặng xong". Ba kết cục còn lại đều là "chặng không hoàn thành", và + // chính sách hàng đợi (`clear_queue_on_failure`) nằm ở mission layer chứ không ở đây. + // + // kPreempted đáng chú ý: mission layer thường đã chuyển sang chặng khác rồi, nên + // onNavigationFailed sẽ trả false và không làm gì — đúng ý, chứ không phải bị bỏ sót. + const bool accepted = (outcome == NavigationOutcome::kSucceeded) + ? manager->onNavigationDone(mission_sequence_id) + : manager->onNavigationFailed(mission_sequence_id); + + if (!accepted) + { + std::lock_guard lock(mutex_); + ++stale_outcomes_; + } +} + +bool MissionAdapterBridge::hasActiveMission() const +{ + mission_adapters::MissionManager* manager = nullptr; + bool has_pending = false; + { + std::lock_guard lock(mutex_); + manager = manager_; + has_pending = static_cast(pending_); + } + + if (has_pending) + { + return true; + } + return manager != nullptr && manager->hasMission(); +} + +void MissionAdapterBridge::start() +{ + std::lock_guard lock(mutex_); + running_ = true; +} + +void MissionAdapterBridge::stop() +{ + std::lock_guard lock(mutex_); + running_ = false; + // Bỏ chặng đang chờ: nó sẽ không bao giờ được chạy, và giữ lại chỉ để nó chạy sau một lần start() + // sau này là hành vi không ai mong đợi. + pending_.reset(); +} + +std::size_t MissionAdapterBridge::droppedRequests() const +{ + std::lock_guard lock(mutex_); + return dropped_requests_; +} + +std::size_t MissionAdapterBridge::staleOutcomes() const +{ + std::lock_guard lock(mutex_); + return stale_outcomes_; +} + +} // namespace move_base2 diff --git a/src/config/move_base2_config.cpp b/src/config/move_base2_config.cpp new file mode 100644 index 0000000..327a8a0 --- /dev/null +++ b/src/config/move_base2_config.cpp @@ -0,0 +1,261 @@ +/********************************************************************* + * move_base2 — đọc và validate cấu hình runtime. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +#include + +namespace move_base2 +{ +namespace +{ + +/// Đọc một khoá double; thiếu khoá thì giữ nguyên default và nói rõ khoá nào bị thiếu. +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); + return; + } + nh.param(key, value, value); +} + +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); + return; + } + nh.param(key, value, value); +} + +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(), + value ? "true" : "false"); + return; + } + nh.param(key, value, value); +} + +void readString(robot::NodeHandle& nh, const std::string& key, std::string& value) +{ + if (!nh.hasParam(key)) + { + robot::log_warning("[move_base2] thiếu param '%s', dùng default '%s'", key.c_str(), + value.c_str()); + return; + } + nh.param(key, value, value); +} + +/// Đọc một binding profile từ namespace con cùng tên. +void readBinding(robot::NodeHandle& nh, const std::string& ns, ProfileBinding& binding) +{ + 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); +} + +bool validateBinding(const ProfileBinding& binding, const char* name, std::string& 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. + 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; +} + +/// Đọc cấu hình đường vào cảm biến từ namespace con `sensors`. +void readSensors(robot::NodeHandle& nh, SensorGatewayConfig& sensors) +{ + robot::NodeHandle sensors_nh(nh, "sensors"); + readBool(sensors_nh, "laser_sor_enabled", sensors.laser_sor_enabled); + readInt(sensors_nh, "laser_sor_mean_k", sensors.laser_sor_mean_k); + readDouble(sensors_nh, "laser_sor_stddev_mul", sensors.laser_sor_stddev_mul); +} + +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"; +} + +} // 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); + + 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); + + // 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. +} + +bool MoveBase2Config::validate(std::string& error) const +{ + if (!std::isfinite(controller_frequency) || controller_frequency <= 0.0) + { + error = "controller_frequency phải > 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"; + 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)"; + return false; + } + if (!std::isfinite(planner_timeout)) + { + error = "planner_timeout không hữu hạn [s]"; + return false; + } + + if (recovery_namespace.empty()) + { + error = "recovery_namespace rỗng"; + return false; + } + if (global_frame.empty() || robot_base_frame.empty()) + { + error = "global_frame và robot_base_frame không được rỗng"; + 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ạ độ"; + return false; + } + + if (!validateBinding(position, "position", error) || + !validateBinding(docking, "docking", error) || + !validateBinding(go_straight, "go_straight", error) || + !validateBinding(rotate, "rotate", error)) + { + return false; + } + + 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"; + return false; + } + + if (!velocity.validate(error)) + { + return false; + } + + if (!sensors.validate(error)) + { + return false; + } + + // Sau cùng: struct con. Thứ tự này có chủ đích — báo lỗi ở tầng cụ thể nhất trước, để thông báo + // nói đúng khoá YAML mà người vận hành cần sửa, chứ không phải một ràng buộc phái sinh. + // + // Lưu ý ràng buộc thứ tự KHỞI TẠO: `state_machine.recovery_behavior_count` KHÔNG đến từ YAML mà + // là số behavior RecoveryRunner nạp được thật. Bên gọi phải điền nó trước khi gọi hàm này — + // xem @ref MoveBase2Config::validate trong header. + if (!state_machine.validate(error)) + { + return false; + } + + return true; +} + +std::string MoveBase2Config::describe() const +{ + std::ostringstream out; + out << "move_base2 config:\n"; + out << " controller_frequency: " << controller_frequency << " Hz\n"; + out << " planner_frequency: " << planner_frequency << " Hz\n"; + out << " planner_timeout: " << planner_timeout << " s\n"; + out << " frames: global='" << global_frame << "' base='" << robot_base_frame << "'\n"; + out << " namespaces: recovery='" << recovery_namespace << "' actions='" << action_namespace + << "' mission='" << mission_namespace << "'\n"; + describeBinding(out, "position", position); + describeBinding(out, "docking", docking); + describeBinding(out, "go_straight", go_straight); + describeBinding(out, "rotate", rotate); + out << state_machine.describe(); + out << velocity.describe(); + out << sensors.describe(); + return out.str(); +} + +ControlLoopConfig MoveBase2Config::toControlLoopConfig() const +{ + ControlLoopConfig config; + config.state_machine = state_machine; + config.velocity = velocity; + config.nominal_control_period = + controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05; // [s] + config.robot_base_frame = robot_base_frame; + config.position = position; + config.docking = docking; + config.go_straight = go_straight; + config.rotate = rotate; + return config; +} + +} // namespace move_base2 diff --git a/src/control_loop.cpp b/src/control_loop.cpp new file mode 100644 index 0000000..8ff1f00 --- /dev/null +++ b/src/control_loop.cpp @@ -0,0 +1,569 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt control loop. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +namespace move_base2 +{ +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; + +} // namespace + +// ================================================================================================ +// ControlLoopConfig +// ================================================================================================ + +bool ControlLoopConfig::validate(std::string& error) const +{ + if (!state_machine.validate(error)) + { + return false; + } + if (!velocity.validate(error)) + { + return false; + } + if (!(nominal_control_period > 0.0)) + { + error = "nominal_control_period phải > 0 [s]"; + return false; + } + if (position.local_planner_name.empty()) + { + error = "profile 'position' bắt buộc phải có local_planner_name"; + 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"; + return false; + } + return true; +} + +std::string ControlLoopConfig::describe() const +{ + std::ostringstream out; + out << state_machine.describe(); + out << velocity.describe(); + out << "ControlLoop:\n"; + out << " nominal_control_period: " << nominal_control_period << " s\n"; + out << " robot_base_frame : " << robot_base_frame << '\n'; + out << " profile position : " << position.global_planner_name << " / " + << position.local_planner_name << '\n'; + out << " profile docking : " << docking.global_planner_name << " / " + << docking.local_planner_name << '\n'; + out << " profile go_straight: " << go_straight.global_planner_name << " / " + << go_straight.local_planner_name << '\n'; + out << " profile rotate : " << rotate.global_planner_name << " / " + << rotate.local_planner_name << '\n'; + return out.str(); +} + +// ================================================================================================ +// ControlLoop +// ================================================================================================ + +bool ControlLoop::configure(const ControlLoopConfig& config, const ControlLoopDeps& deps, + std::string& error) +{ + initialized_ = false; + + 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)"; + return false; + } + if (!config.validate(error)) + { + return false; + } + if (!state_machine_.configure(config.state_machine, error)) + { + return false; + } + if (!arbiter_.configure(config.velocity, error)) + { + return false; + } + + config_ = config; + deps_ = deps; + initialized_ = true; + reset(); + return true; +} + +void ControlLoop::reset() +{ + state_machine_.reset(); + arbiter_.reset(); + + has_pending_request_ = false; + has_active_request_ = false; + pause_requested_ = false; + resume_requested_ = false; + cancel_requested_ = false; + + planner_feedback_ = PlannerFeedback::kIdle; + controller_feedback_ = ControllerFeedback::kIdle; + recovery_feedback_ = RecoveryFeedback::kIdle; + action_feedback_ = ActionFeedback::kIdle; + + latest_plan_.clear(); + planner_running_ = 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_; + if (deps_.planner != nullptr) + { + deps_.planner->cancelPlan(); + } + + has_last_cycle_time_ = false; + has_oscillation_origin_ = false; + + has_outcome_ = false; + outcome_report_count_ = 0; + last_reason_ = ""; +} + +const ProfileBinding* ControlLoop::bindingFor(MotionProfile profile) const +{ + switch (profile) + { + case MotionProfile::kPosition: + return &config_.position; + case MotionProfile::kDocking: + return &config_.docking; + case MotionProfile::kGoStraight: + return &config_.go_straight; + case MotionProfile::kRotate: + return &config_.rotate; + } + return nullptr; +} + +bool ControlLoop::isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose) +{ + const auto& q = pose.pose.orientation; + if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)) + { + return false; + } + const double norm_sq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + return std::abs(std::sqrt(norm_sq) - 1.0) <= kQuaternionNormTolerance; +} + +bool ControlLoop::submit(const NavigationRequest& request, std::string& reason) +{ + if (!initialized_) + { + reason = "runtime chưa khởi tạo"; + 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"; + return false; + } + + if (!request.has_goal) + { + // 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"; + return false; + } + pending_request_ = request; + has_pending_request_ = true; + cancel_requested_ = false; + return true; + } + + 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"; + return false; + } + if (!isQuaternionValid(request.goal)) + { + reason = "goal có quaternion không hợp lệ"; + return false; + } + + const ProfileBinding* binding = bindingFor(request.profile); + if (binding == nullptr || binding->local_planner_name.empty()) + { + reason = std::string("chưa cấu hình planner cho profile '") + toString(request.profile) + "'"; + 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 + "'"; + return false; + } + if (!deps_.controller->swapPlanner(binding->local_planner_name)) + { + reason = "không nạp được 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); + + pending_request_ = request; + has_pending_request_ = true; + + // 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; + return true; +} + +void ControlLoop::requestPause() +{ + pause_requested_ = true; + resume_requested_ = false; +} + +void ControlLoop::requestResume() +{ + resume_requested_ = true; + pause_requested_ = false; +} + +void ControlLoop::requestCancel() +{ + cancel_requested_ = true; +} + +void ControlLoop::collectPlannerResult() +{ + PlanResult result; + if (!deps_.planner->pollPlan(result)) + { + // Chưa có kết quả. Chỉ báo "đang lập" khi thật sự có lượt đang chạy VÀ chưa có phản hồi nào + // khác — `apply_plan` ở cycle trước có thể đã đặt kFailed khi controller từ chối plan, và đó + // là tin quan trọng hơn. + if (planner_running_ && planner_feedback_ == PlannerFeedback::kIdle) + { + planner_feedback_ = PlannerFeedback::kBusy; + } + return; + } + + planner_running_ = false; + + if (result.tag != plan_tag_) + { + // Kết quả của một yêu cầu đã bị thay hoặc huỷ. Vứt lặng lẽ: bám theo nó nghĩa là robot đi tới + // goal không còn ai yêu cầu. Không phải lỗi, nên cũng không đặt kFailed. + return; + } + + // Contract của PlannerPort là "succeeded nghĩa là plan không rỗng". Vẫn kiểm lại ở đây vì plan + // 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()) + { + planner_feedback_ = PlannerFeedback::kFailed; + return; + } + + latest_plan_.swap(result.plan); + planner_feedback_ = PlannerFeedback::kPlanReady; +} + +void ControlLoop::runController(robot_geometry_msgs::Twist& candidate) +{ + candidate = robot_geometry_msgs::Twist(); + + // Giữ nguyên thứ tự của interface được bọc: hỏi đã tới đích trước, chỉ khi chưa mới tính lệnh. + if (deps_.controller->isGoalReached()) + { + controller_feedback_ = ControllerFeedback::kGoalReached; + return; + } + + robot_geometry_msgs::Twist cmd; + if (deps_.controller->computeVelocityCommands(cmd)) + { + controller_feedback_ = ControllerFeedback::kCommandValid; + candidate = cmd; + return; + } + + controller_feedback_ = ControllerFeedback::kNoValidCommand; +} + +bool ControlLoop::step() +{ + if (!initialized_) + { + return false; + } + + // --- 1. Thời gian và pose ------------------------------------------------------------------- + const robot::Time now = deps_.clock->now(); + + double dt = config_.nominal_control_period; + if (has_last_cycle_time_) + { + dt = (now - last_cycle_time_).toSec(); + if (dt < 0.0) + { + // Đồng hồ lùi (thường do đổi nguồn time). Không có dt tin được thì bỏ giới hạn gia tốc ở + // cycle này thay vì tính ra một giá trị bịa. + dt = 0.0; + } + } + last_cycle_time_ = now; + has_last_cycle_time_ = true; + + // Thu kết quả lập plan TRƯỚC khi dựng dữ liệu vào: state machine quyết định dựa trên phản hồi, + // nên phản hồi phải có mặt trước lúc nó chạy. + collectPlannerResult(); + + robot_geometry_msgs::PoseStamped robot_pose; + const bool pose_available = deps_.pose->getRobotPose(robot_pose); + + double travelled = 0.0; + if (pose_available && has_oscillation_origin_) + { + const double dx = robot_pose.pose.position.x - oscillation_origin_.pose.position.x; + const double dy = robot_pose.pose.position.y - oscillation_origin_.pose.position.y; + travelled = std::hypot(dx, dy); + } + + // --- 2. State machine ----------------------------------------------------------------------- + StateMachineInput input; + input.now = now; + input.has_pending_request = has_pending_request_; + input.pending_request_has_goal = pending_request_.has_goal; + input.pending_request_action_count = pending_request_.actions.size(); + input.pause_requested = pause_requested_; + input.resume_requested = resume_requested_; + input.cancel_requested = cancel_requested_; + input.planner = planner_feedback_; + input.controller = controller_feedback_; + input.recovery = recovery_feedback_; + input.action = action_feedback_; + input.pose_available = pose_available; + input.robot_stopped = arbiter_.stopped(); + input.travelled_since_oscillation_reset = travelled; + + // Cùng một chỉ số dùng cho cả cycle khởi động recovery lẫn các cycle tick: state machine chỉ tăng + // chỉ số khi behavior kết thúc, nên `nextRecoveryIndex()` chính là behavior sắp chạy hoặc đang + // chạy. Hỏi trước khi state machine quyết định để nó biết có nên trao quyền phát vận tốc không. + input.active_recovery_output = + deps_.recovery != nullptr + ? deps_.recovery->outputKind(state_machine_.nextRecoveryIndex()) + : RecoveryOutputKind::kNone; + + const StateMachineOutput output = state_machine_.update(input); + + if (output.state_changed) + { + last_reason_ = output.reason; + } + + // Cờ một-lần đã được state machine tiêu thụ xong ở lời gọi trên. + pause_requested_ = false; + resume_requested_ = false; + + // Phản hồi của cycle trước đã dùng xong; đặt lại để cycle này tự sinh phản hồi mới. + planner_feedback_ = PlannerFeedback::kIdle; + controller_feedback_ = ControllerFeedback::kIdle; + recovery_feedback_ = RecoveryFeedback::kIdle; + action_feedback_ = ActionFeedback::kIdle; + + // --- 3. Thi hành output --------------------------------------------------------------------- + if (output.accept_request) + { + active_request_ = pending_request_; + has_active_request_ = true; + has_pending_request_ = false; + latest_plan_.clear(); + has_outcome_ = false; + + // Nhãn mới: mọi lượt lập plan đang bay thuộc về goal cũ và phải bị vứt khi về. + ++plan_tag_; + deps_.planner->cancelPlan(); + planner_running_ = false; + } + + if (output.reset_oscillation_origin && pose_available) + { + oscillation_origin_ = robot_pose; + has_oscillation_origin_ = true; + } + + if (output.stop_planner) + { + // Huỷ thật, không chỉ quên đi: nếu chỉ hạ cờ thì lượt đang chạy vẫn về và chiếm chỗ hộp thư, + // rồi bị nhận nhầm cho lượt kế tiếp mang cùng nhãn. + deps_.planner->cancelPlan(); + planner_running_ = false; + } + + if (output.apply_plan && !latest_plan_.empty()) + { + if (!deps_.controller->setPlan(latest_plan_)) + { + // Controller từ chối plan: coi như lần lập plan này hỏng, để chu kỳ kiên nhẫn tiếp tục chạy. + planner_feedback_ = PlannerFeedback::kFailed; + } + } + + if (output.cancel_recovery) + { + deps_.recovery->cancel(); + } + + robot_geometry_msgs::Twist candidate; + + if (output.start_recovery) + { + if (!deps_.recovery->start(output.recovery_index, output.recovery_trigger)) + { + // Behavior từ chối khởi động (ví dụ đã va chạm ngay tại chỗ) — báo hỏng để state machine + // chuyển sang behavior kế tiếp thay vì tick một behavior chưa start. + recovery_feedback_ = RecoveryFeedback::kFailed; + } + } + else if (output.tick_recovery) + { + const RecoveryTick tick = deps_.recovery->update(); + switch (tick.status) + { + case RecoveryTick::Status::kRunning: + recovery_feedback_ = RecoveryFeedback::kRunning; + break; + case RecoveryTick::Status::kSucceeded: + recovery_feedback_ = RecoveryFeedback::kSucceeded; + break; + case RecoveryTick::Status::kFailed: + recovery_feedback_ = RecoveryFeedback::kFailed; + break; + } + if (tick.has_velocity) + { + candidate = tick.cmd; + } + if (tick.has_path && !tick.path.empty()) + { + // Họ recovery sinh lại đường đi: coi kết quả như một plan mới, chờ state machine đẩy xuống. + latest_plan_ = tick.path; + planner_feedback_ = PlannerFeedback::kPlanReady; + } + } + + if (output.cancel_action && deps_.action != nullptr) + { + deps_.action->cancel(); + } + + if (output.start_action) + { + // Cửa submit đã chặn yêu cầu có action mà không có port, nhưng vẫn guard: kẹt ở đây nghĩa là + // contract bị phá từ một đường khác — báo action hỏng để state machine kết thúc tường minh. + if (deps_.action == nullptr || output.action_index >= active_request_.actions.size()) + { + action_feedback_ = ActionFeedback::kFailed; + } + else if (!deps_.action->start(active_request_.actions[output.action_index])) + { + // Không có handler cho actionType này hoặc handler từ chối — action coi như thất bại. + action_feedback_ = ActionFeedback::kFailed; + } + } + else if (output.tick_action && deps_.action != nullptr) + { + const ActionTick tick = deps_.action->update(); + switch (tick.status) + { + case ActionTick::Status::kRunning: + action_feedback_ = ActionFeedback::kRunning; + break; + case ActionTick::Status::kSucceeded: + action_feedback_ = ActionFeedback::kSucceeded; + break; + case ActionTick::Status::kFailed: + action_feedback_ = ActionFeedback::kFailed; + break; + } + } + + if (output.run_controller) + { + runController(candidate); + } + + if (output.start_planner && pose_available && !planner_running_) + { + // `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_); + 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ồ + // kiên nhẫn tiếp tục chạy, thay vì đứng im ở kPlanning vô hạn. + planner_feedback_ = PlannerFeedback::kFailed; + } + } + + // --- 4. Lệnh vận tốc ------------------------------------------------------------------------ + arbiter_.arbitrate(output.velocity_source, candidate, dt); + + // --- 5. Báo kết quả ------------------------------------------------------------------------- + if (output.report_outcome) + { + last_outcome_ = output.outcome; + has_outcome_ = true; + ++outcome_report_count_; + + if (deps_.mission != nullptr && has_active_request_ && + active_request_.mission_sequence_id != 0) + { + deps_.mission->reportOutcome(active_request_.mission_sequence_id, output.outcome); + } + + has_active_request_ = false; + cancel_requested_ = false; + latest_plan_.clear(); + planner_running_ = false; + } + + return !output.report_outcome; +} + +const char* ControlLoop::lastOutcome() const +{ + return has_outcome_ ? toString(last_outcome_) : ""; +} + +} // namespace move_base2 diff --git a/src/io/sensor_gateway.cpp b/src/io/sensor_gateway.cpp new file mode 100644 index 0000000..14cb0ab --- /dev/null +++ b/src/io/sensor_gateway.cpp @@ -0,0 +1,337 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt cửa vào dữ liệu cảm biến. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +#include +#include +#include +#include + +namespace move_base2 +{ +namespace +{ + +/// [s] Giãn cách log cho các cảnh báo phát sinh trong đường nóng — tần số cảm biến, không được spam. +constexpr double kHotPathLogThrottle = 5.0; + +const char* toString(robot_costmap_2d::LayerType type) +{ + switch (type) + { + case robot_costmap_2d::LayerType::STATIC_LAYER: + return "StaticLayer"; + case robot_costmap_2d::LayerType::OBSTACLE_LAYER: + return "ObstacleLayer"; + case robot_costmap_2d::LayerType::VOXEL_LAYER: + return "VoxelLayer"; + case robot_costmap_2d::LayerType::INFLATION_LAYER: + return "InflationLayer"; + case robot_costmap_2d::LayerType::CRITICAL_LAYER: + return "CriticalLayer"; + case robot_costmap_2d::LayerType::DIRECTIONAL_LAYER: + return "DirectionalLayer"; + case robot_costmap_2d::LayerType::PREFERRED_LAYER: + return "PreferredLayer"; + case robot_costmap_2d::LayerType::UNPREFERRED_LAYER: + return "UnpreferredLayer"; + case robot_costmap_2d::LayerType::UNKNOWN: + break; + } + return "Unknown"; +} + +} // namespace + +// ================================================================================================ +// SensorGatewayConfig +// ================================================================================================ + +bool SensorGatewayConfig::validate(std::string& error) const +{ + // Chỉ kiểm khi lọc bật: tham số của một tính năng đang tắt không nên chặn được cả runtime. + if (!laser_sor_enabled) + { + return true; + } + + if (laser_sor_mean_k < 2) + { + error = "laser_sor_mean_k phải >= 2 [điểm] khi 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"; + return false; + } + return true; +} + +std::string SensorGatewayConfig::describe() const +{ + std::ostringstream out; + out << "SensorGateway:\n"; + 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_stddev_mul : " << laser_sor_stddev_mul << '\n'; + } + return out.str(); +} + +// ================================================================================================ +// SensorGateway +// ================================================================================================ + +SensorGateway::SensorGateway() = default; + +// Định nghĩa ở đây (không phải `= default` trong header) vì laser_sor_ là unique_ptr tới kiểu chưa +// hoàn chỉnh ở phía header. +SensorGateway::~SensorGateway() = default; + +bool SensorGateway::configure(const SensorGatewayConfig& config, std::string& error) +{ + if (!config.validate(error)) + { + return false; + } + + config_ = config; + + if (config_.laser_sor_enabled) + { + // Dựng một lần, không phải mỗi mẫu như bản cũ. Tham số cũng chỉ set ở đây. + laser_sor_.reset(new laser_filter::LaserScanSOR()); + laser_sor_->setMeanK(config_.laser_sor_mean_k); + laser_sor_->setStddevMulThresh(config_.laser_sor_stddev_mul); + } + else + { + laser_sor_.reset(); + } + + configured_ = true; + return true; +} + +void SensorGateway::attach(robot_costmap_2d::LayeredCostmap* global, + robot_costmap_2d::LayeredCostmap* local) +{ + global_costmap_ = global; + local_costmap_ = local; + + warnAboutUnreachableLayers(global_costmap_, "global"); + warnAboutUnreachableLayers(local_costmap_, "local"); +} + +bool SensorGateway::attached() const +{ + return global_costmap_ != nullptr || local_costmap_ != nullptr; +} + +void SensorGateway::warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap* costmap, + const char* which) const +{ + if (costmap == nullptr) + { + return; + } + + auto* plugins = costmap->getPlugins(); + if (plugins == nullptr) + { + return; + } + + // Dữ liệu vật cản được đẩy theo LayerType::VOXEL_LAYER, đúng như bản cũ. Một layer khai + // `type: ObstacleLayer` thuần sẽ không bao giờ khớp và không nhận được gì — im lặng. Đây là bẫy + // có thật: cây config `robot_costmap_2d/config/costmap_params.yaml` đang khai đúng kiểu đó. + // Không tự ý mở rộng đích để tránh đổi hành vi; thay vào đó nói ra lúc khởi tạo. + for (const auto& layer : *plugins) + { + if (!layer) + { + continue; + } + 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", + which, layer->getName().c_str()); + } + } +} + +robot_sensor_msgs::LaserScan SensorGateway::prepareLaserScan( + const robot_sensor_msgs::LaserScan& scan) const +{ + if (!config_.laser_sor_enabled || laser_sor_ == nullptr) + { + return scan; + } + return laser_sor_->filter(scan); +} + +namespace +{ + +/** + * @brief Đưa một mẫu tới mọi layer khớp kiểu trong một costmap. + * + * Template nằm trong .cpp có chủ đích: nó là chỗ duy nhất chạm `dataCallBack`, và giữ nó ở đây làm + * cho `sensor_gateway.h` không phải kéo theo `robot_costmap_2d`. + */ +template +void dispatchTo(robot_costmap_2d::LayeredCostmap* costmap, const T& value, + robot_costmap_2d::LayerType type, const std::string& name, + SensorGatewayStats& stats) +{ + if (costmap == nullptr) + { + return; + } + + auto* plugins = costmap->getPlugins(); + if (plugins == nullptr) + { + return; + } + + for (const auto& layer : *plugins) + { + if (!layer) + { + continue; + } + + // Lọc CHỈ theo kiểu. Vế `|| getName() == name` của bản cũ bị bỏ — xem doc của lớp. + if (layer->getType() != type) + { + continue; + } + + if (!layer->isEnabled()) + { + ++stats.skipped_disabled; + continue; + } + + try + { + layer->dataCallBack(value, name); + ++stats.delivered; + } + catch (const std::exception& ex) + { + // Bắt quanh TỪNG layer: bản cũ bắt quanh cả vòng lặp rồi return, nên một layer hỏng làm mọi + // layer đứng sau nó mất luôn mẫu này. + ++stats.layer_exceptions; + robot::log_error_throttle( + kHotPathLogThrottle, + "[SensorGateway] layer '%s' (%s) ném exception khi nhận '%s': %s\n", + layer->getName().c_str(), toString(type), name.c_str(), ex.what()); + } + } +} + +} // namespace + +void SensorGateway::pushStaticMap(const std::string& name, const robot_nav_msgs::OccupancyGrid& map) +{ + if (!attached()) + { + ++stats_.dropped_no_costmap; + robot::log_warning_throttle(kHotPathLogThrottle, + "[SensorGateway] bỏ static map '%s': chưa gắn costmap nào\n", + name.c_str()); + return; + } + + dispatchTo(global_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_); + dispatchTo(local_costmap_, map, robot_costmap_2d::LayerType::STATIC_LAYER, name, stats_); +} + +void SensorGateway::pushLaserScan(const std::string& name, const robot_sensor_msgs::LaserScan& scan) +{ + if (!attached()) + { + ++stats_.dropped_no_costmap; + robot::log_warning_throttle(kHotPathLogThrottle, + "[SensorGateway] bỏ laser scan '%s': chưa gắn costmap nào\n", + name.c_str()); + return; + } + + dispatchTo(local_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); + dispatchTo(global_costmap_, scan, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); +} + +void SensorGateway::pushPointCloud(const std::string& name, + const robot_sensor_msgs::PointCloud& cloud) +{ + if (!attached()) + { + ++stats_.dropped_no_costmap; + robot::log_warning_throttle(kHotPathLogThrottle, + "[SensorGateway] bỏ point cloud '%s': chưa gắn costmap nào\n", + name.c_str()); + return; + } + + dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); + dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); +} + +void SensorGateway::pushPointCloud2(const std::string& name, + const robot_sensor_msgs::PointCloud2& cloud) +{ + if (!attached()) + { + ++stats_.dropped_no_costmap; + robot::log_warning_throttle(kHotPathLogThrottle, + "[SensorGateway] bỏ point cloud2 '%s': chưa gắn costmap nào\n", + name.c_str()); + return; + } + + dispatchTo(local_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); + dispatchTo(global_costmap_, cloud, robot_costmap_2d::LayerType::VOXEL_LAYER, name, stats_); +} + +void SensorGateway::pushDepthCameraData(const std::string& topic, + const robot_sensor_msgs::DepthCameraData::ConstPtr& data) +{ + if (data == nullptr) + { + return; + } + + if (!attached()) + { + ++stats_.dropped_no_costmap; + robot::log_warning_throttle(kHotPathLogThrottle, + "[SensorGateway] bỏ depth camera '%s': chưa gắn costmap nào\n", + topic.c_str()); + return; + } + + // 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_); + dispatchTo(global_costmap_, data, robot_costmap_2d::LayerType::VOXEL_LAYER, topic, stats_); +} + +} // namespace move_base2 diff --git a/src/move_base2_plugin.cpp b/src/move_base2_plugin.cpp new file mode 100644 index 0000000..7a98034 --- /dev/null +++ b/src/move_base2_plugin.cpp @@ -0,0 +1,35 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — export plugin Boost.DLL. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include + +#include + +namespace move_base2 +{ + +/** + * @brief Factory được host nạp qua boost::dll::import_alias. + * + * Kiểu trả về phải khớp CHÍNH XÁC kiểu mà loader khai báo: + * `robot::move_base_core::BaseNavigation::Ptr()`. Cơ chế nạp là dlsym + reinterpret_cast, không có + * kiểm kiểu nào qua ranh giới .so — lệch kiểu ở đây không gây lỗi biên dịch mà gây hỏng bộ nhớ lúc + * chạy. Đổi contract thì phải đổi cả loader trong cùng một lần sửa. + */ +robot::move_base_core::BaseNavigation::Ptr createMoveBase2() +{ + return std::make_shared(); +} + +} // namespace move_base2 + +BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase2) diff --git a/src/navigation_server.cpp b/src/navigation_server.cpp new file mode 100644 index 0000000..595ab38 --- /dev/null +++ b/src/navigation_server.cpp @@ -0,0 +1,653 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt facade BaseNavigation. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include + +namespace move_base2 +{ +namespace +{ + +/// @brief Lấy phần tử theo khoá, trả về giá trị mặc định nếu không có. +template +typename MapT::mapped_type lookupOrDefault(const MapT& container, + const typename MapT::key_type& key) +{ + const auto it = container.find(key); + return it == container.end() ? typename MapT::mapped_type() : it->second; +} + +} // namespace + +NavigationServer::NavigationServer() +{ + // nav_feedback_ là thành viên của contract host và được host đọc qua con trỏ, nên phải tồn tại + // 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_->goal_checked = false; + nav_feedback_->is_ready = false; +} + +NavigationServer::~NavigationServer() = default; + +// ================================================================================================ +// Cấu hình lõi +// ================================================================================================ + +bool NavigationServer::configureLoop(const ControlLoopConfig& config, const ControlLoopDeps& deps, + std::string& error) +{ + if (!loop_.configure(config, deps, error)) + { + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "cấu hình lỗi: " + error; + return false; + } + + robot_base_frame_ = config.robot_base_frame; + + nav_feedback_->is_ready = true; + nav_feedback_->feed_back_str = "sẵn sàng"; + refreshFeedback(); + return true; +} + +bool NavigationServer::configureSensors(const SensorGatewayConfig& config, std::string& error) +{ + return sensors_.configure(config, error); +} + +void NavigationServer::attachCostmaps(robot_costmap_2d::LayeredCostmap* global, + robot_costmap_2d::LayeredCostmap* local) +{ + sensors_.attach(global, local); + + // Phát lại static map đã nhận trước khi costmap tồn tại. Chỉ static map, cố ý: một laser scan cũ + // phát lại vào costmap là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu — im lặng và nguy + // hiểm hơn hẳn việc chờ mẫu kế tiếp, vốn chỉ cách vài chục ms. + std::map maps; + { + std::lock_guard lock(data_mutex_); + maps = static_maps_; + + // `map_save_`/`map_name_save_` là cặp biến PUBLIC của BaseNavigation mà host tự gán (đường bù + // của bản cũ cho đúng vấn đề này). Tôn trọng nó để host không phải sửa gì, nhưng không để nó + // ghi đè bản đã đi qua addStaticMap. + if (!map_name_save_.empty() && maps.find(map_name_save_) == maps.end()) + { + maps[map_name_save_] = map_save_; + } + } + + for (const auto& entry : maps) + { + sensors_.pushStaticMap(entry.first, entry.second); + } +} + +bool NavigationServer::spinOnce() +{ + // 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ạ. + pushHostInputsToController(); + + const bool running = loop_.step(); + publishCommand(); + refreshFeedback(); + return running; +} + +void NavigationServer::publishCommand() +{ + // getTwist() của contract host là LỆNH vận tốc đang phát, không phải vận tốc đo được: host lấy nó + // rồi publish thẳng ra cmd_vel (amr_publiser.cpp:360-370). Đổ odometry vào đây tạo vòng lặp dương + // — robot giữ nguyên tốc độ hiện tại vô hạn và VelocityArbiter bị vô hiệu hoàn toàn. + // + // Nguồn duy nhất đúng là lệnh vừa qua bộ trọng tài. Dấu thời gian lấy theo cycle của control loop + // chứ không phải giờ hệ thống lúc gọi: host loại lệnh quá hạn, nên control loop treo phải làm dấu + // thời gian đứng yên để host thấy được và ngừng phát. + const robot_geometry_msgs::Twist& command = loop_.lastCommand(); + + std::lock_guard lock(data_mutex_); + twist_.velocity = robot_nav_2d_utils::twist3Dto2D(command); + twist_.header.stamp = loop_.lastCycleTime(); + twist_.header.frame_id = robot_base_frame_; +} + +robot::move_base_core::State NavigationServer::toHostState(NavigationState state) +{ + using HostState = robot::move_base_core::State; + switch (state) + { + case NavigationState::kIdle: + return HostState::PENDING; + case NavigationState::kPlanning: + return HostState::PLANNING; + case NavigationState::kControlling: + return HostState::CONTROLLING; + case NavigationState::kRecovering: + // Contract host chỉ có CLEARING cho giai đoạn phục hồi. Ánh xạ về đó để host cũ không phải + // đổi gì; ngữ nghĩa mới (recovery có thời lượng, có thể phát vận tốc) nằm ở phía lõi. + return HostState::CLEARING; + case NavigationState::kExecutingActions: + // Contract host không có khái niệm action (D8 mới thêm). ACTIVE (actionlib: goal đang được + // xử lý) là ánh xạ đúng: giữ nghĩa "chặng chưa xong" nhưng KHÔNG phải CONTROLLING — host + // VDA5050 đang suy `driving = true` từ CONTROLLING (amr_vda_5050_client_api.cpp:1233), mà + // robot lúc này đứng yên làm action; báo "đang chạy" cho fleet master là báo sai. + return HostState::ACTIVE; + case NavigationState::kPaused: + return HostState::PAUSED; + case NavigationState::kCancelling: + return HostState::PREEMPTING; + case NavigationState::kSucceeded: + return HostState::SUCCEEDED; + case NavigationState::kAborted: + return HostState::ABORTED; + case NavigationState::kCancelled: + return HostState::PREEMPTED; + } + return HostState::LOST; +} + +void NavigationServer::refreshFeedback() +{ + nav_feedback_->navigation_state = toHostState(loop_.state()); + + const char* reason = loop_.lastReason(); + if (reason != nullptr && reason[0] != '\0') + { + nav_feedback_->feed_back_str = reason; + } + + robot_geometry_msgs::Pose2D pose2d; + nav_feedback_->goal_checked = getRobotPose(pose2d); + if (nav_feedback_->goal_checked) + { + nav_feedback_->current_pose = pose2d; + } +} + +// ================================================================================================ +// Khởi tạo +// ================================================================================================ + +void NavigationServer::initialize(robot::TFListenerPtr tf) +{ + tf_ = tf; + + // Phần dựng costmap, planner runner, controller runner và recovery runner từ tf này thuộc bước + // nối dây runtime. Cho tới lúc đó, các cổng phải được bơm vào qua configureLoop() — cố ý KHÔNG + // tự dựng cổng giả ở đây, vì một runtime chạy được với cổng giả là thứ nguy hiểm nhất có thể có. + if (!loop_.initialized()) + { + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "đã nhận tf, chờ configureLoop() nạp các cổng runtime"; + } +} + +// ================================================================================================ +// Footprint +// ================================================================================================ + +void NavigationServer::setRobotFootprint(const std::vector& fprt) +{ + std::lock_guard lock(data_mutex_); + footprint_ = fprt; +} + +std::vector NavigationServer::getRobotFootprint() +{ + std::lock_guard lock(data_mutex_); + return footprint_; +} + +// ================================================================================================ +// Nhận dữ liệu sensor +// ================================================================================================ + +// Khuôn chung của cả năm hàm dưới đây: cất giữ dưới `data_mutex_`, ĐÓNG lock, rồi mới đẩy vào +// costmap. Thứ tự đó là bắt buộc chứ không phải phong cách — `StaticLayer::incomingMap` có thể gọi +// `LayeredCostmap::resizeMap`, hàm này chờ mutex master costmap và có thể đứng trọn một chu kỳ +// `updateMap`. Đẩy trong lúc còn giữ `data_mutex_` sẽ kéo theo `getTwist`/`getRobotFootprint`/ +// `getStaticMap` của host chết chờ cùng, trong khi host chỉ có MỘT thread phục vụ mọi callback. + +void NavigationServer::addStaticMap(const std::string& map_name, robot_nav_msgs::OccupancyGrid map) +{ + { + std::lock_guard lock(data_mutex_); + static_maps_[map_name] = map; + } + sensors_.pushStaticMap(map_name, map); +} + +void NavigationServer::addLaserScan(const std::string& laser_scan_name, + robot_sensor_msgs::LaserScan laser_scan) +{ + // Lọc TRƯỚC khi cất: bản được cất và bản costmap nhìn thấy phải là một. Bản cũ cũng cất bản đã + // lọc; nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật sẽ lệch nhau. + const robot_sensor_msgs::LaserScan prepared = sensors_.prepareLaserScan(laser_scan); + { + std::lock_guard lock(data_mutex_); + laser_scans_[laser_scan_name] = prepared; + } + sensors_.pushLaserScan(laser_scan_name, prepared); +} + +void NavigationServer::addPointCloud(const std::string& point_cloud_name, + robot_sensor_msgs::PointCloud point_cloud) +{ + { + std::lock_guard lock(data_mutex_); + point_clouds_[point_cloud_name] = point_cloud; + } + sensors_.pushPointCloud(point_cloud_name, point_cloud); +} + +void NavigationServer::addPointCloud2(const std::string& point_cloud2_name, + robot_sensor_msgs::PointCloud2 point_cloud2) +{ + { + std::lock_guard lock(data_mutex_); + point_cloud2s_[point_cloud2_name] = point_cloud2; + } + sensors_.pushPointCloud2(point_cloud2_name, point_cloud2); +} + +void NavigationServer::addDepthCameraData(const std::string& topic, + robot_sensor_msgs::DepthCameraData::ConstPtr data) +{ + if (data == nullptr) + { + return; // Bỏ mẫu null thay vì cất một con trỏ rỗng để tầng sau vấp phải. + } + { + std::lock_guard lock(data_mutex_); + depth_camera_data_[topic] = data; + } + sensors_.pushDepthCameraData(topic, data); +} + +void NavigationServer::addOdometry(const std::string& /*odometry_name*/, + robot_nav_msgs::Odometry odometry) +{ + // CHỈ cất odometry. Không đụng twist_: xem publishCommand() — twist_ là lệnh phát ra, còn đây là + // vận tốc đo được. Trộn hai thứ đó là đưa cảm biến vào thẳng đường lệnh. + std::lock_guard lock(data_mutex_); + odometry_ = std::move(odometry); +} + +// ================================================================================================ +// Đọc dữ liệu sensor +// ================================================================================================ + +robot_nav_msgs::OccupancyGrid NavigationServer::getStaticMap(const std::string& map_name) +{ + std::lock_guard lock(data_mutex_); + return lookupOrDefault(static_maps_, map_name); +} + +robot_sensor_msgs::LaserScan NavigationServer::getLaserScan(const std::string& laser_scan_name) +{ + std::lock_guard lock(data_mutex_); + return lookupOrDefault(laser_scans_, laser_scan_name); +} + +robot_sensor_msgs::PointCloud NavigationServer::getPointCloud(const std::string& point_cloud_name) +{ + std::lock_guard lock(data_mutex_); + return lookupOrDefault(point_clouds_, point_cloud_name); +} + +robot_sensor_msgs::PointCloud2 NavigationServer::getPointCloud2( + const std::string& point_cloud2_name) +{ + std::lock_guard lock(data_mutex_); + return lookupOrDefault(point_cloud2s_, point_cloud2_name); +} + +std::map NavigationServer::getAllStaticMaps() +{ + std::lock_guard lock(data_mutex_); + return static_maps_; +} + +std::map NavigationServer::getAllLaserScans() +{ + std::lock_guard lock(data_mutex_); + return laser_scans_; +} + +std::map NavigationServer::getAllPointClouds() +{ + std::lock_guard lock(data_mutex_); + return point_clouds_; +} + +std::map NavigationServer::getAllPointCloud2s() +{ + std::lock_guard lock(data_mutex_); + return point_cloud2s_; +} + +bool NavigationServer::removeStaticMap(const std::string& map_name) +{ + std::lock_guard lock(data_mutex_); + return static_maps_.erase(map_name) > 0; +} + +bool NavigationServer::removeLaserScan(const std::string& laser_scan_name) +{ + std::lock_guard lock(data_mutex_); + return laser_scans_.erase(laser_scan_name) > 0; +} + +bool NavigationServer::removePointCloud(const std::string& point_cloud_name) +{ + std::lock_guard lock(data_mutex_); + return point_clouds_.erase(point_cloud_name) > 0; +} + +bool NavigationServer::removePointCloud2(const std::string& point_cloud2_name) +{ + std::lock_guard lock(data_mutex_); + return point_cloud2s_.erase(point_cloud2_name) > 0; +} + +bool NavigationServer::removeAllStaticMaps() +{ + std::lock_guard lock(data_mutex_); + static_maps_.clear(); + return true; +} + +bool NavigationServer::removeAllLaserScans() +{ + std::lock_guard lock(data_mutex_); + laser_scans_.clear(); + return true; +} + +bool NavigationServer::removeAllPointClouds() +{ + std::lock_guard lock(data_mutex_); + point_clouds_.clear(); + return true; +} + +bool NavigationServer::removeAllPointCloud2s() +{ + std::lock_guard lock(data_mutex_); + point_cloud2s_.clear(); + return true; +} + +bool NavigationServer::removeAllData() +{ + std::lock_guard lock(data_mutex_); + static_maps_.clear(); + laser_scans_.clear(); + point_clouds_.clear(); + point_cloud2s_.clear(); + depth_camera_data_.clear(); + return true; +} + +// ================================================================================================ +// Sáu entry point di chuyển -> một NavigationRequest +// ================================================================================================ + +bool NavigationServer::submit(const NavigationRequest& request) +{ + std::string reason; + if (loop_.submit(request, reason)) + { + last_reject_reason_.clear(); + return true; + } + + last_reject_reason_ = reason; + nav_feedback_->feed_back_str = "từ chối yêu cầu: " + reason; + return false; +} + +bool NavigationServer::moveTo(const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + NavigationRequest request; + request.profile = MotionProfile::kPosition; + request.goal = goal; + request.tolerance.xy = xy_goal_tolerance; + request.tolerance.yaw = yaw_goal_tolerance; + return submit(request); +} + +bool NavigationServer::moveTo(const robot_protocol_msgs::Order& msg, + const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + 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); +} + +bool NavigationServer::dockTo(const std::string& maker, + const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double 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); +} + +bool NavigationServer::dockTo(const robot_protocol_msgs::Order& msg, const std::string& marker, + const robot_geometry_msgs::PoseStamped& goal, + double xy_goal_tolerance, double 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); +} + +bool NavigationServer::moveStraightTo(const robot_geometry_msgs::PoseStamped& goal, + double 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) +{ + NavigationRequest request; + request.profile = MotionProfile::kRotate; + request.goal = goal; + request.tolerance.yaw = yaw_goal_tolerance; + return submit(request); +} + +// ================================================================================================ +// Điều khiển vòng đời +// ================================================================================================ + +void NavigationServer::pause() +{ + loop_.requestPause(); +} + +void NavigationServer::resume() +{ + loop_.requestResume(); +} + +void NavigationServer::cancel() +{ + loop_.requestCancel(); +} + +bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear) +{ + // Không phải lệnh jog dù tên nghe như vậy: đây là TRẦN vận tốc, dấu chọn chiều, và host truyền + // xuống đây tốc độ đã bị tầng an toàn hạ (amr_control.cpp:561, 671-680). + if (!std::isfinite(linear.x) || !std::isfinite(linear.y) || !std::isfinite(linear.z)) + { + return false; + } + + // Chỉ cất lại. Host gọi từ thread của nó (OPC-UA/VDA5050/ROS) còn ControllerPort không + // thread-safe, nên việc đẩy xuống controller thuộc về spinOnce() — xem pushHostInputsToController. + std::lock_guard lock(data_mutex_); + if (linear.x < 0.0) + { + pending_linear_backward_ = linear; + has_pending_linear_backward_ = true; + } + else + { + pending_linear_forward_ = linear; + has_pending_linear_forward_ = true; + } + return true; +} + +bool NavigationServer::setTwistAngular(const robot_geometry_msgs::Vector3& angular) +{ + if (!std::isfinite(angular.x) || !std::isfinite(angular.y) || !std::isfinite(angular.z)) + { + return false; + } + + std::lock_guard lock(data_mutex_); + pending_angular_ = angular; + has_pending_angular_ = true; + return true; +} + +void NavigationServer::pushHostInputsToController() +{ + ControllerPort* controller = loop_.controllerPort(); + if (controller == nullptr) + { + return; + } + + robot_geometry_msgs::Vector3 linear_forward; + robot_geometry_msgs::Vector3 linear_backward; + robot_geometry_msgs::Vector3 angular; + bool push_forward = false; + bool push_backward = false; + bool push_angular = false; + robot_geometry_msgs::Twist velocity; + + { + std::lock_guard lock(data_mutex_); + + push_forward = has_pending_linear_forward_; + push_backward = has_pending_linear_backward_; + push_angular = has_pending_angular_; + linear_forward = pending_linear_forward_; + linear_backward = pending_linear_backward_; + angular = pending_angular_; + has_pending_linear_forward_ = false; + has_pending_linear_backward_ = false; + has_pending_angular_ = false; + + velocity = odometry_.twist.twist; + } + + // Đẩy xuống NGOÀI lock: controller là plugin bên thứ ba, thời gian chạy của nó không được phép + // chặn các getter mà host đang gọi. + if (push_forward) + { + controller->setTwistLinear(linear_forward); + } + if (push_backward) + { + controller->setTwistLinear(linear_backward); + } + if (push_angular) + { + controller->setTwistAngular(angular); + } + + controller->setMeasuredVelocity(velocity); +} + +// ================================================================================================ +// Đọc trạng thái +// ================================================================================================ + +bool NavigationServer::getRobotPose(robot_geometry_msgs::PoseStamped& pose) +{ + PosePort* port = loop_.posePort(); + if (port == nullptr) + { + return false; + } + // Hỏi thẳng cổng mỗi lần, không cache: mất TF phải nhìn thấy được ngay tại lời gọi này chứ không + // phải nhận về một pose cũ đã hết hạn. + return port->getRobotPose(pose); +} + +bool NavigationServer::getRobotPose(robot_geometry_msgs::Pose2D& pose) +{ + robot_geometry_msgs::PoseStamped stamped; + if (!getRobotPose(stamped)) + { + return false; + } + const robot_nav_2d_msgs::Pose2DStamped converted = + robot_nav_2d_utils::poseStampedToPose2D(stamped); + pose = converted.pose; + return true; +} + +robot_nav_2d_msgs::Twist2DStamped NavigationServer::getTwist() +{ + std::lock_guard lock(data_mutex_); + return twist_; +} + +robot::move_base_core::NavFeedback* NavigationServer::getFeedback() +{ + return nav_feedback_.get(); +} + +robot::move_base_core::PlannerDataOutput NavigationServer::getGlobalData() +{ + return global_data_; +} + +robot::move_base_core::PlannerDataOutput NavigationServer::getLocalData() +{ + return local_data_; +} + +} // namespace move_base2 diff --git a/src/navigation_state.cpp b/src/navigation_state.cpp new file mode 100644 index 0000000..e16535e --- /dev/null +++ b/src/navigation_state.cpp @@ -0,0 +1,135 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — tên state và phân loại state. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include + +namespace move_base2 +{ + +const char* toString(NavigationState state) +{ + switch (state) + { + case NavigationState::kIdle: + return "IDLE"; + case NavigationState::kPlanning: + return "PLANNING"; + case NavigationState::kControlling: + return "CONTROLLING"; + case NavigationState::kRecovering: + return "RECOVERING"; + case NavigationState::kExecutingActions: + return "EXECUTING_ACTIONS"; + case NavigationState::kPaused: + return "PAUSED"; + case NavigationState::kCancelling: + return "CANCELLING"; + case NavigationState::kSucceeded: + return "SUCCEEDED"; + case NavigationState::kAborted: + return "ABORTED"; + case NavigationState::kCancelled: + return "CANCELLED"; + } + return "UNKNOWN"; +} + +bool isTerminal(NavigationState state) +{ + return state == NavigationState::kSucceeded || state == NavigationState::kAborted || + state == NavigationState::kCancelled; +} + +bool mustBeStopped(NavigationState state) +{ + // Chỉ hai state được phép có vận tốc khác 0: kControlling (local planner lái) và kRecovering + // (recovery behavior lái). Mọi state còn lại là hàng rào an toàn — kể cả kExecutingActions: + // theo D8 action chạy khi robot đứng yên, action cần chuyển động phải là motion profile. + return state != NavigationState::kControlling && state != NavigationState::kRecovering; +} + +const char* toString(MotionProfile profile) +{ + switch (profile) + { + case MotionProfile::kPosition: + return "position"; + case MotionProfile::kDocking: + return "docking"; + case MotionProfile::kGoStraight: + return "go_straight"; + case MotionProfile::kRotate: + return "rotate"; + } + return "unknown"; +} + +const char* toString(NavigationOutcome outcome) +{ + switch (outcome) + { + case NavigationOutcome::kSucceeded: + return "SUCCEEDED"; + case NavigationOutcome::kFailed: + return "FAILED"; + case NavigationOutcome::kCancelled: + return "CANCELLED"; + case NavigationOutcome::kPreempted: + return "PREEMPTED"; + } + return "UNKNOWN"; +} + +const char* toString(RecoveryTrigger trigger) +{ + switch (trigger) + { + case RecoveryTrigger::kPlanningFailed: + return "planning_failed"; + case RecoveryTrigger::kControllingFailed: + return "controlling_failed"; + case RecoveryTrigger::kOscillation: + return "oscillation"; + } + return "unknown"; +} + +const char* toString(RecoveryOutputKind kind) +{ + switch (kind) + { + case RecoveryOutputKind::kNone: + return "none"; + case RecoveryOutputKind::kVelocity: + return "velocity"; + case RecoveryOutputKind::kPath: + return "path"; + } + return "unknown"; +} + +const char* toString(VelocitySource source) +{ + switch (source) + { + case VelocitySource::kNone: + return "none"; + case VelocitySource::kController: + return "controller"; + case VelocitySource::kRecovery: + return "recovery"; + } + return "unknown"; +} + +} // namespace move_base2 diff --git a/src/runners/action_runner.cpp b/src/runners/action_runner.cpp new file mode 100644 index 0000000..232dfe6 --- /dev/null +++ b/src/runners/action_runner.cpp @@ -0,0 +1,302 @@ +/********************************************************************* + * move_base2 — hiện thực ActionPort bằng các ActionHandler plugin. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include +#include +#include + +#include + +namespace move_base2 +{ + +ActionRunner::~ActionRunner() +{ + // Handler phải chết TRƯỚC factory: factory là thứ giữ .so còn nạp. + active_ = nullptr; + by_type_.clear(); + handlers_.clear(); + factories_.clear(); +} + +void ActionRunner::setClock(ClockPort* clock) +{ + clock_ = clock; +} + +void ActionRunner::setNamespace(const std::string& ns) +{ + namespace_ = ns; +} + +bool ActionRunner::registerHandler(const ActionHandler::Ptr& handler) +{ + 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; +} + +ActionHandler* ActionRunner::find(const std::string& action_type) const +{ + const auto it = by_type_.find(action_type); + return it == by_type_.end() ? nullptr : it->second; +} + +std::vector ActionRunner::supportedActionTypes() const +{ + std::vector types; + types.reserve(by_type_.size()); + for (const auto& entry : by_type_) + { + types.push_back(entry.first); + } + return types; +} + +bool ActionRunner::loadOne(const std::string& name, const std::string& type, + robot::NodeHandle& nh, const std::string& ns) +{ + robot::PluginLoaderHelper loader(nh); + const std::string library_path = loader.findLibraryPath(type); + + if (library_path.empty()) + { + robot::log_error("[move_base2] ActionRunner: không tìm được thư viện cho '%s' — kiểm khoá " + "'%s/library_path' trong YAML và sự tồn tại của file .so.", + type.c_str(), type.c_str()); + return false; + } + + std::function 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; +} + +bool ActionRunner::configure(robot::NodeHandle& nh) +{ + if (configured_) + { + robot::log_error("[move_base2] ActionRunner: configure() gọi lần thứ hai."); + return false; + } + + if (clock_ == nullptr) + { + robot::log_error("[move_base2] ActionRunner: thiếu ClockPort — handler không có mốc timeout."); + 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; + } + } + + configured_ = true; + return all_ok; +} + +bool ActionRunner::start(const robot_protocol_msgs::Action& action) +{ + active_ = nullptr; + active_action_id_.clear(); + + if (!configured_) + { + robot::log_error("[move_base2] ActionRunner: start() trước configure()."); + return false; + } + + if (action.actionType.empty()) + { + robot::log_error("[move_base2] ActionRunner: action không có actionType."); + return false; + } + + ActionHandler* handler = 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()); + 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').", + action.actionType.c_str(), action.actionId.c_str()); + return false; + } + + active_ = handler; + active_action_id_ = action.actionId; + return true; +} + +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. + tick.status = ActionTick::Status::kFailed; + tick.message = "update() khi không có action nào đang chạy"; + return tick; + } + + tick = active_->update(clock_->now()); + + if (tick.status != ActionTick::Status::kRunning) + { + active_ = nullptr; + } + + return tick; +} + +void ActionRunner::cancel() +{ + if (active_ != nullptr) + { + active_->cancel(); + active_ = nullptr; + } +} + +} // namespace move_base2 diff --git a/src/runners/controller_runner.cpp b/src/runners/controller_runner.cpp new file mode 100644 index 0000000..73fa3bb --- /dev/null +++ b/src/runners/controller_runner.cpp @@ -0,0 +1,408 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt ControllerRunner. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace move_base2 +{ +namespace +{ + +/// [s] Giãn cách log cho cảnh báo phát sinh trong đường nóng — nhịp control loop, không được spam. +constexpr double kHotPathLogThrottle = 5.0; + +bool isFiniteTwist(const robot_geometry_msgs::Twist& twist) +{ + return std::isfinite(twist.linear.x) && std::isfinite(twist.linear.y) && + std::isfinite(twist.angular.z); +} + +} // namespace + +ControllerRunner::ControllerRunner() = default; +ControllerRunner::~ControllerRunner() = default; + +bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* tf, + robot_costmap_2d::Costmap2DROBOT* costmap, + const std::string& initial_controller, std::string& error) +{ + if (configured_) + { + error = "ControllerRunner::configure() gọi lần thứ hai"; + return false; + } + + if (costmap == nullptr) + { + error = "ControllerRunner cần costmap local khác null"; + return false; + } + + nh_ = nh; + tf_ = tf; + costmap_ = costmap; + configured_ = true; + + if (!initial_controller.empty() && !swapPlanner(initial_controller)) + { + error = "không nạp được local planner khởi đầu '" + initial_controller + "'"; + configured_ = false; + return false; + } + + return true; +} + +robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& name) +{ + const auto cached = controllers_.find(name); + if (cached != controllers_.end()) + { + return cached->second.instance.get(); + } + + robot::PluginLoaderHelper loader(nh_); + const std::string library_path = loader.findLibraryPath(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", + name.c_str(), name.c_str()); + return nullptr; + } + + Loaded loaded; + + try + { + loaded.factory = boost::dll::import_alias( + library_path, name, boost::dll::load_mode::append_decorations); + } + catch (const boost::system::system_error& ex) + { + robot::log_error("[move_base2] ControllerRunner: không nạp được symbol '%s' từ '%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(), + ex.what()); + return nullptr; + } + + try + { + loaded.instance = loaded.factory(); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] ControllerRunner: factory của '%s' ném 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()); + return nullptr; + } + + try + { + // Khác BaseGlobalPlanner: initialize ở đây trả void, nên không có cách nào biết plugin tự thấy + // mình hỏng. Chỉ chặn được exception. + loaded.instance->initialize(name, tf_, costmap_); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] ControllerRunner: initialize() của '%s' ném exception: %s\n", + name.c_str(), ex.what()); + return nullptr; + } + + const auto inserted = controllers_.emplace(name, std::move(loaded)); + return inserted.first->second.instance.get(); +} + +void ControllerRunner::applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller) +{ + if (controller == nullptr) + { + return; + } + + // Trần vận tốc thuộc về YÊU CẦU, không thuộc về instance planner. Instance mới nạp không biết gì + // về các trần host đã đặt trước đó — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an + // toàn cho phép, và không có dấu hiệu nào cả. + try + { + if (has_limit_linear_forward_) + { + controller->setTwistLinear(limit_linear_forward_); + } + if (has_limit_linear_backward_) + { + controller->setTwistLinear(limit_linear_backward_); + } + if (has_limit_angular_) + { + controller->setTwistAngular(limit_angular_); + } + } + 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()); + } +} + +bool ControllerRunner::swapPlanner(const std::string& planner_name) +{ + if (!configured_) + { + robot::log_error("[move_base2] ControllerRunner: swapPlanner() trước configure().\n"); + return false; + } + + if (planner_name.empty()) + { + robot::log_error("[move_base2] ControllerRunner: tên controller rỗng.\n"); + return false; + } + + if (planner_name == active_name_ && active_ != nullptr) + { + return true; // Đã đúng controller; không log để khỏi spam ở cửa vào mỗi yêu cầu. + } + + robot_nav_core::BaseLocalPlanner* controller = acquire(planner_name); + if (controller == nullptr) + { + // Giữ nguyên controller đang chạy: bên gọi từ chối yêu cầu dựa vào giá trị trả về. + return false; + } + + active_ = controller; + active_name_ = planner_name; + applyPendingLimits(active_); + + robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%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"); + return false; + } + + if (plan.empty()) + { + // Plan rỗng lọt xuống sẽ thành front()/back() trên vector rỗng bên trong planner. + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: từ chối plan rỗng.\n"); + return false; + } + + try + { + return active_->setPlan(plan); + } + catch (const std::exception& ex) + { + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: '%s' ném exception trong setPlan: " + "%s\n", active_name_.c_str(), ex.what()); + return false; + } +} + +bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd) +{ + cmd = robot_geometry_msgs::Twist(); + + if (!configured_ || active_ == nullptr) + { + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: computeVelocityCommands() khi chưa " + "có controller.\n"); + return false; + } + + robot_geometry_msgs::Twist result; + bool ok = false; + + try + { + ok = active_->computeVelocityCommands(measured_velocity_, result); + } + catch (const std::exception& ex) + { + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: '%s' ném exception khi tính lệnh: " + "%s\n", active_name_.c_str(), ex.what()); + return false; + } + + if (!ok) + { + return false; + } + + if (!isFiniteTwist(result)) + { + // 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", + active_name_.c_str()); + return false; + } + + cmd = result; + return true; +} + +bool ControllerRunner::isGoalReached() +{ + if (!configured_ || active_ == nullptr) + { + return false; + } + + try + { + return active_->isGoalReached(); + } + catch (const std::exception& ex) + { + // 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 " + "isGoalReached: %s\n", active_name_.c_str(), ex.what()); + return false; + } +} + +void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) +{ + if (!isFiniteTwist(velocity)) + { + // 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"); + return; + } + measured_velocity_ = velocity; +} + +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"); + return false; + } + + // Dấu chọn chiều — xem doc của ControllerPort::setTwistLinear. Nhớ cả hai chiều riêng để + // swapPlanner còn áp lại được lên instance mới. + if (linear.x < 0.0) + { + limit_linear_backward_ = linear; + has_limit_linear_backward_ = true; + } + else + { + limit_linear_forward_ = linear; + has_limit_linear_forward_ = true; + } + + if (active_ == nullptr) + { + // Host đặt trần trước khi controller được nạp là chuyện bình thường: thứ tự khởi tạo không do + // move_base2 quyết. Đã nhớ lại, sẽ áp khi có controller. + return true; + } + + try + { + return active_->setTwistLinear(linear); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistLinear: %s\n", + active_name_.c_str(), ex.what()); + return false; + } +} + +bool ControllerRunner::setTwistAngular(const robot_geometry_msgs::Vector3& angular) +{ + 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"); + return false; + } + + limit_angular_ = angular; + has_limit_angular_ = true; + + if (active_ == nullptr) + { + return true; + } + + try + { + return active_->setTwistAngular(angular); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] ControllerRunner: '%s' ném exception trong setTwistAngular: %s\n", + active_name_.c_str(), ex.what()); + return false; + } +} + +std::string ControllerRunner::activeController() const +{ + return active_ != nullptr ? active_name_ : std::string(); +} + +} // namespace move_base2 diff --git a/src/runners/planner_runner.cpp b/src/runners/planner_runner.cpp new file mode 100644 index 0000000..ba8cffe --- /dev/null +++ b/src/runners/planner_runner.cpp @@ -0,0 +1,395 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt PlannerRunner. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace move_base2 +{ +namespace +{ + +/// @brief Pose có dùng được để lập plan không — NaN/Inf lọt vào SBPL là hỏng ở tầng khó truy nhất. +bool isFinitePose(const robot_geometry_msgs::PoseStamped& pose) +{ + return std::isfinite(pose.pose.position.x) && std::isfinite(pose.pose.position.y) && + std::isfinite(pose.pose.orientation.z) && std::isfinite(pose.pose.orientation.w); +} + +} // namespace + +PlannerRunner::PlannerRunner() = default; + +PlannerRunner::~PlannerRunner() +{ + { + std::lock_guard lock(mutex_); + shutdown_ = true; + discard_ = true; + } + cv_.notify_all(); + + if (thread_.joinable()) + { + // Chờ có chủ đích: plugin là hộp đen nạp lúc chạy, không có đường cắt ngang một phép tính đang + // chạy. Detach thay vì join sẽ để thread chạm vào `planning_`/`handoff_` sau khi chúng đã bị + // huỷ — hỏng ở chỗ không thể truy được. + thread_.join(); + } +} + +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"; + return false; + } + + if (costmap == nullptr) + { + // 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"; + return false; + } + + nh_ = nh; + costmap_ = costmap; + configured_ = true; + + if (!initial_planner.empty() && !swapPlanner(initial_planner)) + { + error = "không nạp được global planner khởi đầu '" + initial_planner + "'"; + configured_ = false; + return false; + } + + thread_ = std::thread(&PlannerRunner::threadBody, this); + return true; +} + +// ================================================================================================ +// Nạp plugin — chỉ control thread chạm +// ================================================================================================ + +robot_nav_core::BaseGlobalPlanner* PlannerRunner::acquire(const std::string& name) +{ + const auto cached = planners_.find(name); + if (cached != planners_.end()) + { + return cached->second.instance.get(); + } + + robot::PluginLoaderHelper loader(nh_); + const std::string library_path = loader.findLibraryPath(name); + + 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", + name.c_str(), name.c_str()); + return nullptr; + } + + Loaded loaded; + + try + { + loaded.factory = boost::dll::import_alias( + library_path, name, boost::dll::load_mode::append_decorations); + } + catch (const boost::system::system_error& ex) + { + robot::log_error("[move_base2] PlannerRunner: không nạp được symbol '%s' từ '%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()); + return nullptr; + } + + try + { + loaded.instance = loaded.factory(); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] PlannerRunner: factory của '%s' ném 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()); + return nullptr; + } + + bool initialized = false; + try + { + initialized = loaded.instance->initialize(name, costmap_); + } + catch (const std::exception& ex) + { + robot::log_error("[move_base2] PlannerRunner: initialize() của '%s' ném exception: %s\n", + name.c_str(), ex.what()); + return nullptr; + } + + if (!initialized) + { + // 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()); + return nullptr; + } + + // Chỉ đưa vào cache khi đã khởi tạo xong — cache một instance hỏng nghĩa là mọi lần thử lại sau + // đều nhận lại đúng cái hỏng đó mà không báo gì. + const auto inserted = planners_.emplace(name, std::move(loaded)); + return inserted.first->second.instance.get(); +} + +bool PlannerRunner::swapPlanner(const std::string& planner_name) +{ + if (!configured_) + { + robot::log_error("[move_base2] PlannerRunner: swapPlanner() trước configure().\n"); + return false; + } + + if (planner_name.empty()) + { + robot::log_error("[move_base2] PlannerRunner: tên planner rỗng.\n"); + return false; + } + + if (planner_name == active_name_) + { + std::lock_guard lock(mutex_); + if (active_ != nullptr) + { + return true; // Đã đúng planner; không log để khỏi spam ở cửa vào mỗi yêu cầu. + } + } + + robot_nav_core::BaseGlobalPlanner* planner = acquire(planner_name); + if (planner == nullptr) + { + // Giữ nguyên planner đang chạy. Bên gọi từ chối yêu cầu dựa vào giá trị trả về; chuyển sang + // trạng thái "không có planner" ở đây sẽ làm hỏng luôn cả yêu cầu đang chạy dở. + return false; + } + + { + std::lock_guard lock(mutex_); + // Lượt đang chạy thuộc về planner cũ — kết quả của nó không còn nghĩa. Không cắt ngang được + // phép tính, chỉ đánh dấu vứt kết quả. Con trỏ planner cũ vẫn hợp lệ vì cache không xoá entry. + if (running_ || pending_) + { + discard_ = true; + } + active_ = planner; + } + + active_name_ = planner_name; + robot::log_info("[move_base2] PlannerRunner: global planner đang dùng là '%s'.\n", + planner_name.c_str()); + return true; +} + +std::string PlannerRunner::activePlanner() const +{ + std::lock_guard lock(mutex_); + return active_ != nullptr ? active_name_ : std::string(); +} + +// ================================================================================================ +// Vòng đời một lượt lập plan +// ================================================================================================ + +bool PlannerRunner::startPlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + const robot_protocol_msgs::Order* order, std::uint64_t tag) +{ + if (!configured_) + { + robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() trước 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"); + return false; + } + + std::lock_guard lock(mutex_); + + if (active_ == nullptr) + { + robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: startPlan() khi chưa có planner.\n"); + return false; + } + + if (pending_ || running_) + { + return false; // Đã có lượt đang chạy; bên gọi phải chờ hoặc huỷ trước. + } + + request_start_ = start; + request_goal_ = goal; + // Sao chép Order: con trỏ chỉ hợp lệ trong lời gọi này, còn lượt lập plan sống lâu hơn thế. + request_order_ = (order != nullptr) ? std::make_shared(*order) + : nullptr; + request_tag_ = tag; + + pending_ = true; + discard_ = false; + has_result_ = false; + + cv_.notify_one(); + return true; +} + +bool PlannerRunner::isPlanning() const +{ + std::lock_guard lock(mutex_); + return pending_ || running_; +} + +bool PlannerRunner::pollPlan(PlanResult& result) +{ + std::lock_guard lock(mutex_); + + if (!has_result_) + { + return false; + } + + has_result_ = false; + result.tag = result_tag_; + result.succeeded = result_ok_; + + // Hoán vị chứ không copy: vector cũ của bên gọi quay lại làm hộp thư và giữ nguyên capacity, nên + // ở trạng thái ổn định không có lần cấp phát nào cho việc bàn giao plan. + result.plan.swap(handoff_); + handoff_.clear(); + return true; +} + +void PlannerRunner::cancelPlan() +{ + std::lock_guard lock(mutex_); + if (pending_ || running_) + { + discard_ = true; + } + // Kết quả đã nằm sẵn trong hộp thư cũng bỏ luôn: bên gọi vừa nói nó không còn cần plan này. + has_result_ = false; + handoff_.clear(); +} + +// ================================================================================================ +// Thread planner +// ================================================================================================ + +void PlannerRunner::threadBody() +{ + std::unique_lock lock(mutex_); + + while (true) + { + cv_.wait(lock, [this] { return shutdown_ || pending_; }); + + if (shutdown_) + { + return; + } + + // Chụp lại yêu cầu rồi NHẢ MUTEX trước khi gọi plugin. Đây là điểm mấu chốt của cả mô hình: + // control thread không bao giờ phải chờ một lượt lập plan. + robot_nav_core::BaseGlobalPlanner* planner = active_; + const robot_geometry_msgs::PoseStamped start = request_start_; + const robot_geometry_msgs::PoseStamped goal = request_goal_; + const std::shared_ptr order = request_order_; + const std::uint64_t tag = request_tag_; + + pending_ = false; + running_ = true; + + lock.unlock(); + + planning_.clear(); + bool ok = false; + + try + { + // 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_) + : planner->makePlan(start, goal, planning_); + } + catch (const std::exception& ex) + { + // 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()); + ok = false; + } + catch (...) + { + robot::log_error_throttle(5.0, "[move_base2] PlannerRunner: plugin ném exception lạ khi lập " + "plan.\n"); + ok = false; + } + + if (!ok || planning_.empty()) + { + // Contract của PlannerPort: thành công nghĩa là plan KHÔNG rỗng. Một số plugin trả true kèm + // vector rỗng; quy về thất bại ngay tại đây để tầng trên không gọi front()/back() trên nó. + planning_.clear(); + ok = false; + } + + lock.lock(); + running_ = false; + + if (discard_) + { + // Lượt này đã bị huỷ hoặc planner đã bị đổi giữa chừng. Vứt lặng lẽ — không phải lỗi. + discard_ = false; + planning_.clear(); + continue; + } + + planning_.swap(handoff_); + result_tag_ = tag; + result_ok_ = ok; + has_result_ = true; + } +} + +} // namespace move_base2 diff --git a/src/runners/recovery_runner.cpp b/src/runners/recovery_runner.cpp new file mode 100644 index 0000000..c349424 --- /dev/null +++ b/src/runners/recovery_runner.cpp @@ -0,0 +1,253 @@ +/********************************************************************* + * move_base2 — hiện thực RecoveryPort bằng recovery_core. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include + +namespace move_base2 +{ +namespace +{ + +/// Dịch lý do vào recovery sang enum của recovery_core. `switch` đầy đủ để compiler bắt được ngay +/// khi một bên thêm giá trị mới — đó là toàn bộ lý do không dùng cast số. +recovery_core::RecoveryTrigger toCoreTrigger(RecoveryTrigger trigger) +{ + switch (trigger) + { + case RecoveryTrigger::kPlanningFailed: + return recovery_core::RecoveryTrigger::kPlanningFailed; + case RecoveryTrigger::kControllingFailed: + return recovery_core::RecoveryTrigger::kControllingFailed; + case RecoveryTrigger::kOscillation: + return recovery_core::RecoveryTrigger::kOscillation; + } + return recovery_core::RecoveryTrigger::kUnspecified; +} + +RecoveryOutputKind toPortKind(recovery_core::RecoveryOutputType kind) +{ + switch (kind) + { + case recovery_core::RecoveryOutputType::kNone: + return RecoveryOutputKind::kNone; + case recovery_core::RecoveryOutputType::kVelocity: + return RecoveryOutputKind::kVelocity; + case recovery_core::RecoveryOutputType::kPath: + return RecoveryOutputKind::kPath; + } + return RecoveryOutputKind::kNone; +} + +} // namespace + +void RecoveryRunner::setDeps(const Deps& deps) +{ + deps_ = deps; + pose_bridge_.setPort(deps_.pose); + collision_.setCostmap(deps_.local_costmap); +} + +void RecoveryRunner::setCostmaps(robot_costmap_2d::Costmap2DROBOT* local, + robot_costmap_2d::Costmap2DROBOT* global) +{ + deps_.local_costmap = local; + deps_.global_costmap = global; + collision_.setCostmap(local); +} + +void RecoveryRunner::setNamespace(const std::string& ns) +{ + namespace_ = ns; +} + +void RecoveryRunner::setPlanSource(PlanSource source) +{ + plan_bridge_.setSource(std::move(source)); +} + +void RecoveryRunner::refreshContext() +{ + // Trỏ lại mỗi lượt thay vì tin bản cache từ configure(): con trỏ costmap là non-owning và có thể + // bị thay khi runtime dựng lại costmap. Cache nó chính là nguyên nhân lỗi double-free đã ghi nhận + // trong workspace. + collision_.setCostmap(deps_.local_costmap); + + ctx_.pose = &pose_bridge_; + ctx_.collision = &collision_; + ctx_.plan = &plan_bridge_; + ctx_.local_costmap = deps_.local_costmap; + ctx_.global_costmap = deps_.global_costmap; +} + +bool RecoveryRunner::configure(robot::NodeHandle& nh) +{ + if (configured_) + { + robot::log_error("[move_base2] RecoveryRunner: configure() gọi lần thứ hai."); + return false; + } + + if (deps_.clock == nullptr || deps_.pose == nullptr) + { + robot::log_error("[move_base2] RecoveryRunner: thiếu ClockPort hoặc PosePort."); + return false; + } + + refreshContext(); + + const bool all_ok = registry_.loadFromConfig(nh, namespace_, ctx_); + + 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()); + return false; + } + + configured_ = true; + + if (!all_ok) + { + // 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ỏ.", + registry_.size()); + return false; + } + + robot::log_info("[move_base2] RecoveryRunner: nạp %zu recovery behavior từ '%s'.", + registry_.size(), namespace_.c_str()); + return true; +} + +std::size_t RecoveryRunner::behaviorCount() const +{ + return registry_.size(); +} + +RecoveryOutputKind RecoveryRunner::outputKind(std::size_t index) const +{ + const recovery_core::RecoveryBehavior* behavior = registry_.at(index); + // Index sai -> kNone: không cấp quyền phát vận tốc cho thứ không biết là gì. + return behavior == nullptr ? RecoveryOutputKind::kNone : toPortKind(behavior->outputKind()); +} + +std::string RecoveryRunner::behaviorName(std::size_t index) const +{ + return registry_.nameAt(index); +} + +bool RecoveryRunner::start(std::size_t index, RecoveryTrigger trigger) +{ + active_ = nullptr; + + if (!configured_) + { + robot::log_error("[move_base2] RecoveryRunner: start() trước 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()); + return false; + } + + refreshContext(); + + recovery_core::RecoveryGoal goal; + goal.trigger = toCoreTrigger(trigger); + // Không đặt angle/distance: để behavior dùng default đã cấu hình của nó. Lõi chưa có nguồn thông + // tin nào để chọn góc/quãng tốt hơn config; khi có (ví dụ hình học vật cản), đặt vào đây. + + if (!behavior->start(goal, deps_.clock->now())) + { + robot::log_warning("[move_base2] RecoveryRunner: behavior '%s' từ chối khởi động (%s).", + registry_.nameAt(index).c_str(), toString(trigger)); + return false; + } + + active_ = behavior; + return true; +} + +RecoveryTick RecoveryRunner::update() +{ + RecoveryTick tick; + + if (active_ == nullptr) + { + // 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"; + return tick; + } + + refreshContext(); + return toTick(active_->update(deps_.clock->now())); +} + +void RecoveryRunner::cancel() +{ + if (active_ != nullptr) + { + active_->cancel(); + } +} + +RecoveryTick RecoveryRunner::toTick(const recovery_core::RecoveryResult& result) const +{ + RecoveryTick tick; + + 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: + // Behavior chưa start mà đã bị tick — lỗi thứ tự gọi, không phải trạng thái bình thường. + tick.status = RecoveryTick::Status::kFailed; + break; + case recovery_core::RecoveryStatus::kCancelled: + // Nhánh này KHÔNG đạt tới được với state machine hiện tại: sau cancel() nó chuyển sang + // CANCELLING, nơi tick_recovery bị ép false. Giữ nhánh lại làm hàng rào nếu sau này ai đó nới + // điều kiện tick — bỏ đi thì kCancelled sẽ rơi vào default và im lặng thành kRunning. + tick.status = RecoveryTick::Status::kFailed; + break; + case recovery_core::RecoveryStatus::kFailed: + tick.status = RecoveryTick::Status::kFailed; + break; + } + + if (const robot_geometry_msgs::Twist* cmd = result.velocity()) + { + tick.has_velocity = true; + tick.cmd = *cmd; + } + + if (const robot_nav_msgs::Path* path = result.pathOut()) + { + if (!path->poses.empty()) + { + tick.has_path = true; + tick.path = path->poses; + } + } + + tick.message = result.message; + return tick; +} + +} // namespace move_base2 diff --git a/src/state_machine.cpp b/src/state_machine.cpp new file mode 100644 index 0000000..2cd4178 --- /dev/null +++ b/src/state_machine.cpp @@ -0,0 +1,578 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt state machine. Bảng chuyển đầy đủ, không I/O, không logging. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +namespace move_base2 +{ + +// ================================================================================================ +// StateMachineConfig +// ================================================================================================ + +bool StateMachineConfig::validate(std::string& error) const +{ + // Các ngưỡng thời gian được phép <= 0 với nghĩa "tắt", nên không kiểm dấu ở đây. Thứ phải chặn là + // 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]"; + 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"; + return false; + } + if (planner_patience <= 0.0 && max_planning_retries < 0) + { + // Từ khi lập plan chạy trên thread riêng, hai tham số này là thứ DUY NHẤT phát hiện được planner + // 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"; + 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"; + return false; + } + return true; +} + +std::string StateMachineConfig::describe() const +{ + std::ostringstream out; + out << "StateMachineConfig:\n"; + out << " planner_patience : " << planner_patience << " s" + << (planner_patience > 0.0 ? "" : " (tắt)") << '\n'; + out << " controller_patience : " << controller_patience << " s" + << (controller_patience > 0.0 ? "" : " (tắt)") << '\n'; + out << " oscillation_timeout : " << oscillation_timeout << " s" + << (oscillation_timeout > 0.0 ? "" : " (tắt)") << '\n'; + out << " action_patience : " << action_patience << " s" + << (action_patience > 0.0 ? "" : " (tắt — handler tự timeout)") << '\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'; + out << " recovery_enabled : " << (recovery_enabled ? "true" : "false") << '\n'; + out << " recovery_behavior_cnt : " << recovery_behavior_count << '\n'; + return out.str(); +} + +// ================================================================================================ +// StateMachine +// ================================================================================================ + +bool StateMachine::configure(const StateMachineConfig& config, std::string& error) +{ + if (!config.validate(error)) + { + initialized_ = false; + return false; + } + + config_ = config; + initialized_ = true; + reset(); + return true; +} + +void StateMachine::reset() +{ + state_ = NavigationState::kIdle; + state_before_pause_ = NavigationState::kIdle; + state_entered_at_ = robot::Time(); + last_valid_plan_ = robot::Time(); + last_valid_control_ = robot::Time(); + last_oscillation_reset_ = robot::Time(); + recovery_index_ = 0; + planning_retries_ = 0; + request_has_goal_ = true; + action_count_ = 0; + action_index_ = 0; + action_started_at_ = robot::Time(); +} + +double StateMachine::secondsInState(const robot::Time& now) const +{ + return (now - state_entered_at_).toSec(); +} + +void StateMachine::enter(NavigationState next, const robot::Time& now, const char* reason, + StateMachineOutput& out) +{ + if (next != state_) + { + state_ = next; + state_entered_at_ = now; + out.state_changed = true; + } + out.state = state_; + out.reason = reason; +} + +void StateMachine::beginPlanningCycle(const robot::Time& now) +{ + last_valid_plan_ = now; + planning_retries_ = 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) + { + finish(NavigationState::kAborted, NavigationOutcome::kFailed, now, + "hết recovery behavior khả dụng", out); + return; + } + + out.start_recovery = true; + out.recovery_index = recovery_index_; + out.recovery_trigger = trigger; + enter(NavigationState::kRecovering, now, reason, out); +} + +void StateMachine::finish(NavigationState terminal, NavigationOutcome outcome, + const robot::Time& now, const char* reason, StateMachineOutput& out) +{ + // Cờ này bật đúng tại cycle bước vào state terminal, và state terminal chỉ tồn tại một cycle + // (cycle sau đã về kIdle). Đó là toàn bộ cơ chế giữ bất biến "báo kết quả đúng một lần". + out.report_outcome = true; + out.outcome = outcome; + out.stop_planner = true; + enter(terminal, now, reason, out); +} + +StateMachineOutput StateMachine::update(const StateMachineInput& in) +{ + StateMachineOutput out; + out.state = state_; + out.recovery_index = recovery_index_; + + if (!initialized_) + { + // 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"; + return out; + } + + // State terminal chỉ sống một cycle. Về kIdle ngay đầu cycle kế tiếp để yêu cầu mới được nhận + // 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); + } + + // 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 + // được phát vận tốc — kể cả recovery. Recovery vẫn được tick để nó tự báo lỗi theo contract của + // nó, nhưng lệnh nó sinh ra bị chặn ở phần chốt bất biến cuối hàm. + const ControllerFeedback controller = + in.pose_available ? in.controller : ControllerFeedback::kNoValidCommand; + + switch (state_) + { + // -------------------------------------------------------------------------------------- + case NavigationState::kIdle: + { + if (in.has_pending_request) + { + out.accept_request = true; + recovery_index_ = 0; + request_has_goal_ = in.pending_request_has_goal; + action_count_ = in.pending_request_action_count; + action_index_ = 0; + + if (!request_has_goal_) + { + // D8: yêu cầu chỉ-có-action — không có gì để lập plan, vào thẳng thực thi action. + if (action_count_ == 0) + { + // 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); + break; + } + 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); + break; + } + + out.start_planner = true; + beginPlanningCycle(in.now); + 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); + } + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kPlanning: + { + if (in.cancel_requested) + { + out.stop_planner = true; + enter(NavigationState::kCancelling, in.now, "huỷ khi đang lập plan", 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); + break; + } + + if (in.planner == PlannerFeedback::kPlanReady) + { + out.apply_plan = true; + out.run_controller = true; // Chạy controller ngay trong cycle này, không phí một vòng. + beginPlanningCycle(in.now); + + // Cố ý KHÔNG làm mới last_valid_control_ và last_oscillation_reset_ ở đây. Có plan mới + // không chứng minh được gì về controller: nếu reset thì vòng lặp + // 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); + break; + } + + if (in.planner == PlannerFeedback::kFailed) + { + ++planning_retries_; + } + + const bool retries_exhausted = config_.max_planning_retries >= 0 && + planning_retries_ > config_.max_planning_retries; + const bool patience_exhausted = + config_.planner_patience > 0.0 && + (in.now - last_valid_plan_).toSec() > config_.planner_patience; + + if (retries_exhausted || patience_exhausted) + { + 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); + break; + } + + out.start_planner = true; // Giữ planner chạy tiếp. + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kControlling: + { + if (in.cancel_requested) + { + out.stop_planner = true; + enter(NavigationState::kCancelling, in.now, "huỷ khi đang bám 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); + break; + } + + // Plan mới tới giữa lúc đang bám plan cũ: nhận ngay, vẫn ở kControlling. + if (in.planner == PlannerFeedback::kPlanReady) + { + out.apply_plan = true; + beginPlanningCycle(in.now); + } + + // Đi đủ xa thì không còn bị coi là quẩn — đặt lại cả đồng hồ lẫn mốc đo quãng đường. + if (config_.oscillation_distance > 0.0 && + in.travelled_since_oscillation_reset >= config_.oscillation_distance) + { + last_oscillation_reset_ = in.now; + out.reset_oscillation_origin = true; + } + + if (controller == ControllerFeedback::kGoalReached) + { + if (action_count_ > 0) + { + // D8: tới goal chưa phải là xong — mission còn action phải chạy tại chỗ. Kết quả chỉ + // được báo sau action cuối, để mission layer thấy trọn một chặng nav + action. + out.stop_planner = true; + 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); + break; + } + finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, "đạt goal", out); + break; + } + + if (controller == ControllerFeedback::kCommandValid) + { + last_valid_control_ = in.now; + + if (config_.oscillation_timeout > 0.0 && + (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); + break; + } + + out.run_controller = true; + break; + } + + // Còn lại: kIdle (cycle đầu sau khi vào state) hoặc kNoValidCommand. + if (config_.controller_patience > 0.0 && + (in.now - last_valid_control_).toSec() > config_.controller_patience) + { + out.stop_planner = true; + escalateToRecovery(RecoveryTrigger::kControllingFailed, in.now, + "quá hạn sinh lệnh vận tốc", out); + break; + } + + if (controller == ControllerFeedback::kNoValidCommand && in.pose_available) + { + // Chưa hết kiên nhẫn: quay lại lập plan. Cố ý KHÔNG reset last_valid_control_ ở đây, nếu + // 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", + out); + break; + } + + // Mất pose thì ở nguyên kControlling (vận tốc đã bị ép về 0 ở phần chốt bất biến) cho tới khi + // controller_patience hết hạn. Lập lại plan không giúp được gì khi vấn đề là định vị, và + // nhảy sang kPlanning chỉ làm lý do vào recovery bị ghi nhận sai thành "lập plan hỏng". + + out.run_controller = true; + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kRecovering: + { + if (in.cancel_requested) + { + out.cancel_recovery = true; + enter(NavigationState::kCancelling, in.now, "huỷ khi đang recovery", out); + break; + } + if (in.pause_requested) + { + // Recovery bị huỷ khi tạm dừng: giữ một behavior ở trạng thái dở dang qua một quãng dừng + // 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); + break; + } + + if (in.recovery == RecoveryFeedback::kSucceeded || in.recovery == RecoveryFeedback::kFailed) + { + // 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_; + 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", + out); + break; + } + + out.tick_recovery = true; + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kExecutingActions: + { + if (in.cancel_requested) + { + out.cancel_action = true; + enter(NavigationState::kCancelling, in.now, "huỷ khi đang chạy action", out); + break; + } + if (in.pause_requested) + { + // Khác recovery, action KHÔNG bị huỷ khi tạm dừng: robot đang đứng yên nên không có rủi ro + // 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); + break; + } + + if (in.action == ActionFeedback::kFailed) + { + // 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", + out); + break; + } + + if (in.action == ActionFeedback::kSucceeded) + { + ++action_index_; + if (action_index_ < action_count_) + { + out.start_action = true; + out.action_index = action_index_; + action_started_at_ = in.now; // Trần thời gian tính cho TỪNG action, không cho cả chuỗi. + break; // Vẫn ở kExecutingActions, chuyển sang action kế tiếp. + } + finish(NavigationState::kSucceeded, NavigationOutcome::kSucceeded, in.now, + "action cuối đã xong", out); + break; + } + + // Lưới an toàn cuối cùng cho action treo (handler hỏng, thiết bị câm lặng vĩnh viễn). + // Cơ chế timeout CHÍNH là của từng ActionHandler; lưới này mặc định tắt. + if (config_.action_patience > 0.0 && + (in.now - action_started_at_).toSec() > config_.action_patience) + { + 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); + break; + } + + // Mất pose KHÔNG chặn tick action: robot đứng yên, thao tác thiết bị không cần định vị. + // Vận tốc vẫn bị ép về 0 ở phần chốt bất biến cuối hàm như mọi state phải dừng khác. + out.tick_action = true; + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kPaused: + { + if (in.cancel_requested) + { + enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out); + break; + } + if (in.resume_requested) + { + // Đặt lại toàn bộ đồng hồ kiên nhẫn: một lần tạm dừng dài không được tính là "planner chậm" + // hay "controller hỏng", nếu không thì resume xong là rơi thẳng vào recovery. + beginPlanningCycle(in.now); + last_valid_control_ = in.now; + last_oscillation_reset_ = in.now; + out.reset_oscillation_origin = true; + + if (state_before_pause_ == NavigationState::kControlling) + { + out.run_controller = true; + enter(NavigationState::kControlling, in.now, "tiếp tục bám plan", out); + } + else if (state_before_pause_ == NavigationState::kExecutingActions) + { + // Action không bị huỷ khi tạm dừng nên không start lại — tick tiếp đúng action dở dang. + // Mốc action_patience được gieo lại: một quãng dừng dài không được tính vào trần thời + // 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); + } + else + { + out.start_planner = true; + enter(NavigationState::kPlanning, in.now, "tiếp tục lập plan", out); + } + } + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kCancelling: + { + // Nguồn vận tốc đã là kNone nên bộ trọng tài đang giảm tốc về 0 theo trần gia tốc; trạng thái + // này vì vậy luôn kết thúc sau hữu hạn cycle, không cần thêm timeout. + if (in.robot_stopped) + { + finish(NavigationState::kCancelled, NavigationOutcome::kCancelled, in.now, + "robot đã dừng hẳn", out); + } + break; + } + + // -------------------------------------------------------------------------------------- + case NavigationState::kSucceeded: + case NavigationState::kAborted: + case NavigationState::kCancelled: + // Không tới được: đã chuyển về kIdle ở đầu hàm. + break; + } + + // ------------------------------------------------------------------------------------------ + // Chốt bất biến trước khi trả ra. Ba dòng này là hàng rào an toàn cuối cùng của lõi. + // ------------------------------------------------------------------------------------------ + out.state = state_; + + if (state_ == NavigationState::kControlling) + { + out.velocity_source = VelocitySource::kController; + } + else if (state_ == NavigationState::kRecovering) + { + // Chỉ behavior thật sự lái robot mới được cấp quyền phát vận tốc. Behavior one-shot (đợi, xoá + // costmap) giữ nguồn ở kNone, nên arbiter không phải đổi nguồn hai lần cho một lượt không có + // vận tốc nào — mỗi lần đổi nguồn tốn một cycle zero (mục 1.6). + out.velocity_source = in.active_recovery_output == RecoveryOutputKind::kVelocity + ? VelocitySource::kRecovery + : VelocitySource::kNone; + } + else + { + out.velocity_source = VelocitySource::kNone; + } + + if (!in.pose_available) + { + out.velocity_source = VelocitySource::kNone; + out.run_controller = false; + } + + if (mustBeStopped(state_)) + { + out.velocity_source = VelocitySource::kNone; + out.run_controller = false; + out.tick_recovery = false; + } + + return out; +} + +} // namespace move_base2 diff --git a/src/velocity_arbiter.cpp b/src/velocity_arbiter.cpp new file mode 100644 index 0000000..9deb0dd --- /dev/null +++ b/src/velocity_arbiter.cpp @@ -0,0 +1,245 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt bộ trọng tài vận tốc. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +namespace move_base2 +{ +namespace +{ + +/// @brief Giá trị hữu hạn (không NaN, không Inf). +bool isFinite(double value) +{ + return std::isfinite(value); +} + +/// @brief Đưa @p value về [lower, upper]. Trả true qua @p clamped nếu có cắt. +double clampTo(double value, double lower, double upper, bool& clamped) +{ + if (value < lower) + { + clamped = true; + return lower; + } + if (value > upper) + { + clamped = true; + return upper; + } + return value; +} + +} // namespace + +// ================================================================================================ +// VelocityLimits +// ================================================================================================ + +bool VelocityLimits::validate(std::string& error) const +{ + if (!(max_vel_x > 0.0)) + { + error = "max_vel_x phải > 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"; + return false; + } + if (!(max_vel_theta > 0.0)) + { + error = "max_vel_theta phải > 0 [rad/s]"; + return false; + } + if (!(max_accel_x > 0.0)) + { + error = "max_accel_x phải > 0 [m/s^2]"; + return false; + } + if (!(max_accel_theta > 0.0)) + { + error = "max_accel_theta phải > 0 [rad/s^2]"; + return false; + } + if (zero_velocity_epsilon < 0.0) + { + error = "zero_velocity_epsilon phải >= 0"; + return false; + } + return true; +} + +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_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"; + out << " zero_velocity_epsilon: " << zero_velocity_epsilon << '\n'; + return out.str(); +} + +// ================================================================================================ +// VelocityArbiter +// ================================================================================================ + +robot_geometry_msgs::Twist VelocityArbiter::zeroTwist() +{ + return robot_geometry_msgs::Twist(); +} + +bool VelocityArbiter::configure(const VelocityLimits& limits, std::string& error) +{ + if (!limits.validate(error)) + { + initialized_ = false; + return false; + } + + limits_ = limits; + initialized_ = true; + reset(); + return true; +} + +void VelocityArbiter::reset() +{ + active_source_ = VelocitySource::kNone; + last_command_ = zeroTwist(); + non_finite_rejections_ = 0; + velocity_clamps_ = 0; + acceleration_clamps_ = 0; + handover_cycles_ = 0; +} + +bool VelocityArbiter::stopped() const +{ + return std::abs(last_command_.linear.x) <= limits_.zero_velocity_epsilon && + std::abs(last_command_.linear.y) <= limits_.zero_velocity_epsilon && + std::abs(last_command_.angular.z) <= limits_.zero_velocity_epsilon; +} + +robot_geometry_msgs::Twist VelocityArbiter::sanitize(const robot_geometry_msgs::Twist& candidate) +{ + robot_geometry_msgs::Twist clean; + + // NaN/Inf từ bất kỳ trục nào làm hỏng cả lệnh: không có cách nào "sửa một phần" một lệnh mà bộ + // sinh ra nó đang ở trạng thái hỏng. Trả 0 và đếm lại để tầng trên phát hiện được. + if (!isFinite(candidate.linear.x) || !isFinite(candidate.linear.y) || + !isFinite(candidate.linear.z) || !isFinite(candidate.angular.x) || + !isFinite(candidate.angular.y) || !isFinite(candidate.angular.z)) + { + ++non_finite_rejections_; + return clean; + } + + bool clamped = false; + clean.linear.x = clampTo(candidate.linear.x, limits_.min_vel_x, limits_.max_vel_x, clamped); + clean.angular.z = + clampTo(candidate.angular.z, -limits_.max_vel_theta, limits_.max_vel_theta, clamped); + + // Robot của workspace là phi holonomic ở mức contract cmd_vel: mọi thành phần còn lại bị bỏ, cố + // ý không truyền tiếp để tránh một plugin lạ đẩy ra trục mà tầng dưới không kiểm. + clean.linear.y = 0.0; + clean.linear.z = 0.0; + clean.angular.x = 0.0; + clean.angular.y = 0.0; + + if (clamped) + { + ++velocity_clamps_; + } + return clean; +} + +robot_geometry_msgs::Twist VelocityArbiter::limitAcceleration( + const robot_geometry_msgs::Twist& target, double dt) +{ + if (dt <= 0.0) + { + // Không biết dt thật thì không có cơ sở nào để giới hạn gia tốc. Trả nguyên lệnh đã sanitize + // thay vì bịa ra một chu kỳ danh nghĩa — bịa chính là lớp lỗi mà thiết kế này muốn tránh. + return target; + } + + robot_geometry_msgs::Twist limited = target; + bool clamped = false; + + const double max_dx = limits_.max_accel_x * dt; + const double max_dtheta = limits_.max_accel_theta * dt; + + limited.linear.x = clampTo(target.linear.x, last_command_.linear.x - max_dx, + last_command_.linear.x + max_dx, clamped); + limited.angular.z = clampTo(target.angular.z, last_command_.angular.z - max_dtheta, + last_command_.angular.z + max_dtheta, clamped); + + if (clamped) + { + ++acceleration_clamps_; + } + return limited; +} + +robot_geometry_msgs::Twist VelocityArbiter::arbitrate(VelocitySource source, + const robot_geometry_msgs::Twist& candidate, + double dt) +{ + if (!initialized_) + { + // Guard bắt buộc: chưa configure thì không phát gì cả. + last_command_ = zeroTwist(); + active_source_ = VelocitySource::kNone; + return last_command_; + } + + if (source == VelocitySource::kNone) + { + // Lệnh 0 TỨC THÌ, không giảm tốc dần. Ba lý do, theo thứ tự quan trọng: + // 1. Lệnh vận tốc là giá trị được chốt lại ở tầng dưới. Nếu control loop dừng giữa lúc đang + // giảm tốc dần thì lệnh khác 0 cuối cùng còn nguyên hiệu lực và robot chạy tiếp. + // 2. Bất biến "state phải dừng thì lệnh đúng bằng 0" trở thành kiểm được, không phải "gần 0". + // 3. Giảm tốc theo động học là việc của bộ điều khiển bánh xe, nơi biết tải và ma sát thật. + active_source_ = VelocitySource::kNone; + last_command_ = zeroTwist(); + return last_command_; + } + + // Đổi nguồn: chèn đúng một cycle vận tốc 0 trước khi nguồn mới được phát. Hai bộ sinh lệnh giữ + // trạng thái gia tốc riêng nên nối thẳng chúng lại gây giật. Cycle 0 này cũng là ranh giới rõ + // ràng để soi log khi điều tra sự cố. + if (active_source_ != VelocitySource::kNone && active_source_ != source) + { + ++handover_cycles_; + active_source_ = source; + last_command_ = zeroTwist(); + return last_command_; + } + + active_source_ = source; + last_command_ = limitAcceleration(sanitize(candidate), dt); + return last_command_; +} + +robot_geometry_msgs::Twist VelocityArbiter::emergencyStop() +{ + active_source_ = VelocitySource::kNone; + last_command_ = zeroTwist(); + return last_command_; +} + +} // namespace move_base2 diff --git a/test/action_runner_test.cpp b/test/action_runner_test.cpp new file mode 100644 index 0000000..35e3048 --- /dev/null +++ b/test/action_runner_test.cpp @@ -0,0 +1,363 @@ +/********************************************************************* + * + * move_base2 — kiểm ActionRunner: định tuyến theo actionType, vòng đời tick, và timeout tầng 1. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include + +#include + +#include + +#include "fake_ports.h" + +namespace +{ +using move_base2::ActionHandler; +using move_base2::ActionRunner; +using move_base2::ActionTick; +using move_base2::testing::FakeClockPort; + +robot_protocol_msgs::Action makeAction(const std::string& type, const std::string& id = "a1") +{ + robot_protocol_msgs::Action action; + action.actionType = type; + action.actionId = id; + return action; +} + +/** + * @brief Handler viết hoàn toàn trong test. + * + * Sự tồn tại của nó là bài kiểm tra thật cho tính plugin: thêm một loại action mới mà không sửa + * file nào trong `src/` của gói. + */ +class ScriptedHandler final : public ActionHandler +{ +public: + explicit ScriptedHandler(std::vector types) : types_(std::move(types)) + { + } + + bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override + { + name_ = name; + return configure_ok; + } + + std::vector supportedActionTypes() const override + { + return types_; + } + + bool start(const robot_protocol_msgs::Action& action, const robot::Time& /*now*/) override + { + ++start_count; + last_action_id = action.actionId; + return start_ok; + } + + ActionTick update(const robot::Time& /*now*/) override + { + ++update_count; + ActionTick tick; + tick.status = next_status; + return tick; + } + + void cancel() override + { + ++cancel_count; + } + + bool configure_ok = true; + bool start_ok = true; + ActionTick::Status next_status = ActionTick::Status::kRunning; + + int start_count = 0; + int update_count = 0; + int cancel_count = 0; + std::string last_action_id; + +private: + std::vector types_; + std::string name_; +}; + +struct Rig +{ + Rig() + { + runner.setClock(&clock); + } + + bool load(const std::string& ns) + { + runner.setNamespace(ns); + robot::NodeHandle nh; + return runner.configure(nh); + } + + FakeClockPort clock{1000.0}; + ActionRunner runner; +}; + +// ------------------------------------------------------------------------------------------------ +// Đăng ký thủ công — kiểm định tuyến và vòng đời mà không cần .so +// ------------------------------------------------------------------------------------------------ + +TEST(ActionRunner, RoutesByActionType) +{ + Rig rig; + auto pick = std::make_shared(std::vector{"pick"}); + auto drop = std::make_shared(std::vector{"drop"}); + ASSERT_TRUE(rig.runner.registerHandler(pick)); + ASSERT_TRUE(rig.runner.registerHandler(drop)); + ASSERT_TRUE(rig.load("actions_empty")); + + ASSERT_TRUE(rig.runner.start(makeAction("drop", "d7"))); + + EXPECT_EQ(drop->start_count, 1); + EXPECT_EQ(pick->start_count, 0); + EXPECT_EQ(drop->last_action_id, "d7"); +} + +TEST(ActionRunner, OneHandlerCanTakeSeveralTypes) +{ + Rig rig; + auto handler = std::make_shared(std::vector{"pick", "drop"}); + ASSERT_TRUE(rig.runner.registerHandler(handler)); + ASSERT_TRUE(rig.load("actions_empty")); + + EXPECT_TRUE(rig.runner.start(makeAction("pick"))); + EXPECT_TRUE(rig.runner.start(makeAction("drop"))); + EXPECT_EQ(handler->start_count, 2); +} + +TEST(ActionRunner, RejectsDuplicateActionType) +{ + Rig rig; + ASSERT_TRUE(rig.runner.registerHandler( + std::make_shared(std::vector{"pick"}))); + + // Hai handler cùng nhận một type thì định tuyến phụ thuộc thứ tự nạp — phải từ chối, không ghi đè. + EXPECT_FALSE(rig.runner.registerHandler( + std::make_shared(std::vector{"pick"}))); +} + +TEST(ActionRunner, RejectsHandlerWithoutTypes) +{ + Rig rig; + EXPECT_FALSE(rig.runner.registerHandler(std::make_shared( + std::vector{}))); + EXPECT_FALSE(rig.runner.registerHandler(nullptr)); +} + +TEST(ActionRunner, UnknownActionTypeFailsStart) +{ + Rig rig; + ASSERT_TRUE(rig.runner.registerHandler( + std::make_shared(std::vector{"pick"}))); + ASSERT_TRUE(rig.load("actions_empty")); + + EXPECT_FALSE(rig.runner.start(makeAction("charge"))); +} + +TEST(ActionRunner, EmptyActionTypeFailsStart) +{ + Rig rig; + ASSERT_TRUE(rig.runner.registerHandler( + std::make_shared(std::vector{"pick"}))); + ASSERT_TRUE(rig.load("actions_empty")); + + EXPECT_FALSE(rig.runner.start(makeAction(""))); +} + +TEST(ActionRunner, StartBeforeConfigureFails) +{ + Rig rig; + ASSERT_TRUE(rig.runner.registerHandler( + std::make_shared(std::vector{"pick"}))); + + EXPECT_FALSE(rig.runner.start(makeAction("pick"))); +} + +TEST(ActionRunner, HandlerRefusingStartIsReported) +{ + Rig rig; + auto handler = std::make_shared(std::vector{"pick"}); + handler->start_ok = false; + ASSERT_TRUE(rig.runner.registerHandler(handler)); + ASSERT_TRUE(rig.load("actions_empty")); + + EXPECT_FALSE(rig.runner.start(makeAction("pick"))); + // Không được tick tiếp sau khi start hỏng. + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed); + EXPECT_EQ(handler->update_count, 0); +} + +TEST(ActionRunner, UpdateWithoutActiveActionFailsInsteadOfCrashing) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions_empty")); + + const ActionTick tick = rig.runner.update(); + + EXPECT_EQ(tick.status, ActionTick::Status::kFailed); + EXPECT_FALSE(tick.message.empty()); +} + +TEST(ActionRunner, TicksUntilHandlerFinishes) +{ + Rig rig; + auto handler = std::make_shared(std::vector{"pick"}); + ASSERT_TRUE(rig.runner.registerHandler(handler)); + ASSERT_TRUE(rig.load("actions_empty")); + ASSERT_TRUE(rig.runner.start(makeAction("pick"))); + + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning); + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning); + + handler->next_status = ActionTick::Status::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. + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed); + EXPECT_EQ(handler->update_count, 3); +} + +TEST(ActionRunner, CancelReachesHandlerAndClearsActive) +{ + Rig rig; + auto handler = std::make_shared(std::vector{"pick"}); + ASSERT_TRUE(rig.runner.registerHandler(handler)); + ASSERT_TRUE(rig.load("actions_empty")); + ASSERT_TRUE(rig.runner.start(makeAction("pick"))); + + rig.runner.cancel(); + + EXPECT_EQ(handler->cancel_count, 1); + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kFailed); +} + +TEST(ActionRunner, CancelWithoutActiveActionIsSafe) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions_empty")); + rig.runner.cancel(); + SUCCEED(); +} + +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"; +} + +TEST(ActionRunner, EmptyHandlerListIsValid) +{ + // Hệ không có thiết bị nào: mọi mission đều nav-only, và ControlLoop::submit đã từ chối yêu cầu + // mang action ngay tại cửa. + Rig rig; + EXPECT_TRUE(rig.load("actions_empty")); + EXPECT_EQ(rig.runner.handlerCount(), 0u); + EXPECT_FALSE(rig.runner.start(makeAction("pick"))); +} + +// ------------------------------------------------------------------------------------------------ +// Nạp thật qua Boost.DLL — đúng đường runtime đi +// ------------------------------------------------------------------------------------------------ + +TEST(ActionRunner, LoadsHandlerPluginFromConfig) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions")); + + ASSERT_EQ(rig.runner.handlerCount(), 1u); + EXPECT_NE(rig.runner.find("wait"), nullptr); + EXPECT_NE(rig.runner.find("pick"), nullptr); + EXPECT_EQ(rig.runner.find("charge"), nullptr); +} + +TEST(ActionRunner, LoadedHandlerRunsForConfiguredDuration) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions")); + ASSERT_TRUE(rig.runner.start(makeAction("wait", "w1"))); // duration: 2.0 s + + rig.clock.advance(1.0); + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kRunning); + + rig.clock.advance(1.0); + EXPECT_EQ(rig.runner.update().status, ActionTick::Status::kSucceeded); +} + +TEST(ActionRunner, HandlerTimesOutOnItsOwnWithoutStateMachineHelp) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions_slow")); // hang: true, timeout 3 s + ASSERT_TRUE(rig.runner.start(makeAction("wait"))); + + rig.clock.advance(2.0); + ASSERT_EQ(rig.runner.update().status, ActionTick::Status::kRunning); + + rig.clock.advance(1.5); + const ActionTick tick = rig.runner.update(); + + // Timeout TẦNG 1: handler tự chịu trách nhiệm, không dựa vào action_patience (mặc định tắt). + EXPECT_EQ(tick.status, ActionTick::Status::kFailed); + EXPECT_NE(tick.message.find("timeout"), std::string::npos); +} + +TEST(ActionRunner, HangingHandlerWithoutTimeoutIsRejectedAtConfigure) +{ + // Treo + tắt timeout = action chạy vĩnh viễn. Contract cấm, nên phải chặn lúc khởi động. + Rig rig; + EXPECT_FALSE(rig.load("actions_hang_forever")); + EXPECT_EQ(rig.runner.handlerCount(), 0u); +} + +TEST(ActionRunner, MissingLibraryPathFailsConfigure) +{ + Rig rig; + EXPECT_FALSE(rig.load("actions_missing_library")); + EXPECT_EQ(rig.runner.handlerCount(), 0u); +} + +TEST(ActionRunner, HandlerWithTimeoutBelowDurationIsRejectedAtConfigure) +{ + // Cấu hình khiến action LUÔN hỏng vì timeout — phải chặn lúc khởi động, không phải lúc chạy. + Rig rig; + EXPECT_FALSE(rig.load("actions_bad")); + EXPECT_EQ(rig.runner.handlerCount(), 0u); +} + +TEST(ActionRunner, ConfigureTwiceRejected) +{ + Rig rig; + ASSERT_TRUE(rig.load("actions")); + + robot::NodeHandle nh; + EXPECT_FALSE(rig.runner.configure(nh)); +} + +} // namespace + +int main(int argc, char** argv) +{ +#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/config/move_base2_params.yaml b/test/config/move_base2_params.yaml new file mode 100644 index 0000000..b9187be --- /dev/null +++ b/test/config/move_base2_params.yaml @@ -0,0 +1,171 @@ +# Config CHỈ dùng cho test của gói. Bản runtime nằm ở `pnkx_nav_core/config/` (C2). +# +# Chạy test kèm: PNKX_NAV_CORE_CONFIG_DIR=src/AMR_T800/Test/move_base2/test/config + +# --- Tham số runtime, dùng cho config_validation_test ---------------------------------------- +move_base2: + controller_frequency: 20.0 # [Hz] + planner_frequency: 0.0 # [Hz] 0 = chỉ lập plan khi cần + planner_timeout: 5.0 # [s] + + planner_patience: 5.0 # [s] + controller_patience: 15.0 # [s] + oscillation_timeout: 0.0 # [s] 0 = tắt + oscillation_distance: 0.5 # [m] + action_patience: 0.0 # [s] 0 = tắt; lưới cuối, không phải cơ chế timeout chính + max_planning_retries: -1 # < 0 = không giới hạn + recovery_behavior_enabled: true + + max_vel_x: 0.5 # [m/s] tiến + min_vel_x: -0.2 # [m/s] lùi, ÂM + max_vel_theta: 1.0 # [rad/s] + acc_lim_x: 1.0 # [m/s^2] + acc_lim_theta: 2.0 # [rad/s^2] + + global_frame: map + robot_base_frame: base_link + + sensors: + laser_sor_enabled: true # lọc outlier laser TRƯỚC khi vào costmap + laser_sor_mean_k: 8 # [điểm] số láng giềng gần nhất dùng để ước lượng + laser_sor_stddev_mul: 1.5 # [-] ngưỡng = mean + hệ_số * stddev + + recovery_namespace: recovery + action_namespace: actions + mission_namespace: mission_adapters + + 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] + +# --- Cấu hình sai, dùng cho test đường lỗi ----------------------------------------------------- +move_base2_bad_frequency: + controller_frequency: 0.0 # phải bị từ chối + +move_base2_bad_frames: + controller_frequency: 20.0 + global_frame: base_link # trùng robot_base_frame -> pose robot luôn là gốc toạ độ + robot_base_frame: base_link + position: + base_local_planner: TestLocalPlanner + +move_base2_no_planner: + controller_frequency: 20.0 # không profile nào có base_local_planner + +move_base2_bad_sensors: + controller_frequency: 20.0 + position: + base_local_planner: TestLocalPlanner + sensors: + laser_sor_enabled: true + laser_sor_mean_k: 1 # < 2 -> vô nghĩa, phải bị từ chối + +# --- Recovery behavior cho recovery_runner_test ------------------------------------------------ +# +# Chỉ khai behavior họ kNone: họ velocity cần Costmap2DROBOT thật (TF + chuỗi layer) nên được kiểm +# ở tầng tích hợp, không kiểm bằng fake ở đây. +recovery: + behaviors: + - {name: wait_short, type: WaitRecovery} + - {name: wait_long, type: WaitRecovery} + + wait_short: + wait_duration: 1.0 # [s] + wait_long: + wait_duration: 5.0 # [s] + timeout: 3.0 # [s] cố ý NGẮN HƠN wait_duration -> lượt này luôn kết thúc bằng timeout + +recovery_empty: + behaviors: [] + +recovery_missing_library: + behaviors: + - {name: ghost, type: GhostRecovery} + +WaitRecovery: + library_path: librecovery_core_wait_recovery + +# GhostRecovery cố ý KHÔNG khai library_path. + +# --- Action handler cho action_runner_test ----------------------------------------------------- +actions: + handlers: + - {name: noop, type: NoopActionHandler} + noop: + action_types: [wait, pick, drop] + duration: 2.0 # [s] + timeout: 10.0 # [s] tầng 1 — trách nhiệm của chính handler + +actions_slow: + handlers: + - {name: noop, type: NoopActionHandler} + noop: + action_types: [wait] + hang: true # mô phỏng thiết bị không bao giờ trả lời + timeout: 3.0 # [s] handler tự cắt, không chờ state machine + +actions_hang_forever: + handlers: + - {name: noop, type: NoopActionHandler} + noop: + action_types: [wait] + hang: true + timeout: 0.0 # tắt timeout + treo = chạy vĩnh viễn -> phải bị chặn lúc configure + +actions_bad: + handlers: + - {name: noop, type: NoopActionHandler} + noop: + action_types: [wait] + duration: 10.0 # [s] + timeout: 5.0 # [s] <= duration -> action LUÔN hỏng; phải bị chặn lúc configure + +actions_empty: + handlers: [] + +actions_missing_library: + handlers: + - {name: ghost, type: GhostActionHandler} + +NoopActionHandler: + library_path: libmove_base2_noop_action_handler + +# --- Global planner giả cho planner_runner_test ------------------------------------------------- +# +# Bốn alias cùng nằm trong một thư viện; mỗi alias là một hành vi mà PlannerRunner phải xử lý đúng. +TestPlannerOk: + library_path: libmove_base2_test_global_planner +TestPlannerEmptyPlan: + library_path: libmove_base2_test_global_planner +TestPlannerThrowing: + library_path: libmove_base2_test_global_planner +TestPlannerInitFails: + library_path: libmove_base2_test_global_planner + +# TestPlannerMissingLibrary cố ý KHÔNG khai library_path. + +# GhostActionHandler cố ý KHÔNG khai library_path. + +# --- Local planner giả cho controller_runner_test ------------------------------------------------ +TestControllerOk: + library_path: libmove_base2_test_local_planner +TestControllerSecondary: + library_path: libmove_base2_test_local_planner +TestControllerNoCommand: + library_path: libmove_base2_test_local_planner +TestControllerNaN: + library_path: libmove_base2_test_local_planner +TestControllerThrowing: + library_path: libmove_base2_test_local_planner +TestControllerRefusesLimits: + library_path: libmove_base2_test_local_planner + +# TestControllerMissing cố ý KHÔNG khai library_path. diff --git a/test/config_validation_test.cpp b/test/config_validation_test.cpp new file mode 100644 index 0000000..b0aa437 --- /dev/null +++ b/test/config_validation_test.cpp @@ -0,0 +1,245 @@ +/********************************************************************* + * + * move_base2 — kiểm việc đọc và validate cấu hình runtime. + * + * Bản cũ đọc param không kiểm miền giá trị: một `controller_frequency` bằng 0 hay một + * `max_planning_retries` âm đi thẳng vào vòng điều khiển. Ở đây cấu hình sai phải chặn runtime khởi + * động, chứ không phải hiện ra thành hành vi lạ lúc chạy. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +#include + +#include + +namespace +{ +using move_base2::MoveBase2Config; + +MoveBase2Config loadFrom(const std::string& ns) +{ + robot::NodeHandle root; + robot::NodeHandle nh(root, ns); + + MoveBase2Config config; + config.fromNodeHandle(nh); + return config; +} + +/// Điền con số mà RecoveryRunner báo lại sau khi nạp behavior — bước bắt buộc trước validate(). +MoveBase2Config withRecoveryCount(MoveBase2Config config, std::size_t count) +{ + config.state_machine.recovery_behavior_count = count; + return config; +} + +/// Cấu hình tối thiểu hợp lệ, dựng bằng tay (không qua YAML). +MoveBase2Config minimalValid() +{ + MoveBase2Config config; + config.position.local_planner_name = "AnyLocalPlanner"; + config.state_machine.recovery_behavior_count = 1; + return config; +} + +TEST(MoveBase2Config, DefaultsAreValid) +{ + const MoveBase2Config config = minimalValid(); + + std::string error; + EXPECT_TRUE(config.validate(error)) << error; +} + +TEST(MoveBase2Config, RejectsRecoveryEnabledWithNoBehaviorLoaded) +{ + // Ràng buộc thứ tự khởi tạo: RecoveryRunner không nạp được behavior nào mà + // recovery_behavior_enabled vẫn true thì mọi lỗi dẫn thẳng tới ABORTED — chặn ngay lúc khởi động. + MoveBase2Config config = minimalValid(); + config.state_machine.recovery_behavior_count = 0; + config.state_machine.recovery_enabled = true; + + std::string error; + EXPECT_FALSE(config.validate(error)); +} + +TEST(MoveBase2Config, ReadsSensorGatewayBlockFromYaml) +{ + const MoveBase2Config config = loadFrom("move_base2"); + + EXPECT_TRUE(config.sensors.laser_sor_enabled); + EXPECT_EQ(config.sensors.laser_sor_mean_k, 8); + EXPECT_DOUBLE_EQ(config.sensors.laser_sor_stddev_mul, 1.5); +} + +TEST(MoveBase2Config, LaserFilterIsOffWhenTheSensorsBlockIsAbsent) +{ + // Cây config gen-1 không có khoá nào cho bộ lọc. Thiếu khoá phải giữ hành vi host ROS đang chạy — + // tức là KHÔNG lọc — chứ không phải âm thầm bật một bộ lọc lên. + const MoveBase2Config config = loadFrom("move_base2_no_planner"); + + EXPECT_FALSE(config.sensors.laser_sor_enabled); +} + +TEST(MoveBase2Config, RejectsSensorFilterParametersOutOfRange) +{ + const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_sensors"), 1); + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("laser_sor_mean_k"), std::string::npos) << error; +} + +TEST(MoveBase2Config, ReadsEveryGroupFromYaml) +{ + const MoveBase2Config config = loadFrom("move_base2"); + + EXPECT_DOUBLE_EQ(config.controller_frequency, 20.0); + EXPECT_DOUBLE_EQ(config.planner_timeout, 5.0); + + EXPECT_DOUBLE_EQ(config.state_machine.planner_patience, 5.0); + EXPECT_DOUBLE_EQ(config.state_machine.controller_patience, 15.0); + EXPECT_EQ(config.state_machine.max_planning_retries, -1); + EXPECT_TRUE(config.state_machine.recovery_enabled); + + EXPECT_DOUBLE_EQ(config.velocity.max_vel_x, 0.5); + EXPECT_DOUBLE_EQ(config.velocity.min_vel_x, -0.2); + EXPECT_DOUBLE_EQ(config.velocity.max_accel_x, 1.0); + + EXPECT_EQ(config.global_frame, "map"); + EXPECT_EQ(config.robot_base_frame, "base_link"); + EXPECT_EQ(config.recovery_namespace, "recovery"); +} + +TEST(MoveBase2Config, ReadsProfileBindingsFromNestedNamespaces) +{ + const MoveBase2Config config = loadFrom("move_base2"); + + 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); +} + +TEST(MoveBase2Config, LoadedConfigValidates) +{ + const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2"), 2); + + std::string error; + EXPECT_TRUE(config.validate(error)) << error; +} + +TEST(MoveBase2Config, RejectsZeroControllerFrequency) +{ + const MoveBase2Config config = loadFrom("move_base2_bad_frequency"); + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("controller_frequency"), std::string::npos); +} + +TEST(MoveBase2Config, RejectsAbsurdlyHighControllerFrequency) +{ + MoveBase2Config config = minimalValid(); + config.controller_frequency = 5000.0; + + std::string error; + EXPECT_FALSE(config.validate(error)); +} + +TEST(MoveBase2Config, RejectsGlobalFrameEqualToBaseFrame) +{ + const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_bad_frames"), 1); + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("global_frame"), std::string::npos); +} + +TEST(MoveBase2Config, RejectsConfigWithNoLocalPlannerAtAll) +{ + const MoveBase2Config config = withRecoveryCount(loadFrom("move_base2_no_planner"), 1); + + 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); +} + +TEST(MoveBase2Config, PropagatesStateMachineValidationFailure) +{ + MoveBase2Config config = minimalValid(); + config.state_machine.oscillation_timeout = 5.0; + config.state_machine.oscillation_distance = 0.0; + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("oscillation"), std::string::npos); +} + +TEST(MoveBase2Config, PropagatesVelocityValidationFailure) +{ + MoveBase2Config config = minimalValid(); + config.velocity.max_vel_x = -1.0; + + std::string error; + EXPECT_FALSE(config.validate(error)); +} + +TEST(MoveBase2Config, ControlLoopConfigDerivesPeriodFromFrequency) +{ + MoveBase2Config config = minimalValid(); + config.controller_frequency = 25.0; + + const auto loop_config = config.toControlLoopConfig(); + + EXPECT_NEAR(loop_config.nominal_control_period, 0.04, 1e-9); // [s] + EXPECT_EQ(loop_config.position.local_planner_name, "AnyLocalPlanner"); +} + +TEST(MoveBase2Config, RecoveryBehaviorCountIsNotReadFromYaml) +{ + // Số behavior phải là số nạp được THẬT, do RecoveryRunner báo lại. Đọc từ YAML thì một behavior + // hỏng vẫn khiến state machine tin là còn đường phục hồi. + const MoveBase2Config config = loadFrom("move_base2"); + EXPECT_EQ(config.state_machine.recovery_behavior_count, 0u); +} + +TEST(MoveBase2Config, DescribeMentionsEveryGroup) +{ + const std::string text = loadFrom("move_base2").describe(); + + EXPECT_NE(text.find("controller_frequency"), std::string::npos); + EXPECT_NE(text.find("position"), std::string::npos); + EXPECT_NE(text.find("docking"), std::string::npos); + EXPECT_NE(text.find("planner_patience"), std::string::npos); + EXPECT_NE(text.find("max_vel_x"), std::string::npos); +} + +} // namespace + +int main(int argc, char** argv) +{ +#ifdef MOVE_BASE2_TEST_CONFIG_DIR + setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0); +#endif + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/controller_runner_test.cpp b/test/controller_runner_test.cpp new file mode 100644 index 0000000..d2ba9f1 --- /dev/null +++ b/test/controller_runner_test.cpp @@ -0,0 +1,381 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test ControllerRunner: nạp plugin thật qua Boost.DLL, trần vận tốc phải tới được + * plugin, và mọi đường lỗi phải trả về "không có lệnh" chứ không để dữ liệu hỏng đi tiếp. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace +{ +using move_base2::ControllerRunner; + +/// [m/s] Lệnh nền của plugin test khi chưa đặt trần và vận tốc đo được bằng 0. +constexpr double kBaseSpeed = 0.25; +/// [rad/s] +constexpr double kBaseYawRate = 0.40; + +/** + * @brief Con trỏ costmap giả. + * + * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy + * đủ). An toàn ở đây vì `test_local_planner.cpp` không alias nào chạm vào con trỏ này — nó chỉ đi + * qua `initialize()` rồi bị bỏ. Đường có costmap thật thuộc test tích hợp (Phase 5). + */ +robot_costmap_2d::Costmap2DROBOT* dummyCostmap() +{ + static std::uintptr_t placeholder = 0; + return reinterpret_cast(&placeholder); +} + +std::vector makePlan(std::size_t poses = 3) +{ + std::vector plan; + for (std::size_t i = 0; i < poses; ++i) + { + robot_geometry_msgs::PoseStamped pose; + pose.header.frame_id = "map"; + pose.pose.position.x = static_cast(i); // [m] + pose.pose.orientation.w = 1.0; + plan.push_back(pose); + } + return plan; +} + +robot_geometry_msgs::Vector3 vec(double x, double y = 0.0, double z = 0.0) +{ + robot_geometry_msgs::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +robot_geometry_msgs::Twist twist(double vx, double wz = 0.0) +{ + robot_geometry_msgs::Twist t; + t.linear.x = vx; // [m/s] + t.angular.z = wz; // [rad/s] + return t; +} + +class Fixture +{ +public: + explicit Fixture(const std::string& name = "TestControllerOk") + { + robot::NodeHandle nh; + std::string error; + ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error); + error_ = error; + } + + bool ok() const + { + return ok_; + } + + const std::string& error() const + { + return error_; + } + + ControllerRunner runner_; + +private: + bool ok_ = false; + std::string error_; +}; + +} // namespace + +// ================================================================================================ +// Cấu hình và nạp plugin +// ================================================================================================ + +TEST(ControllerRunner, RefusesNullCostmap) +{ + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + + EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error)); + EXPECT_FALSE(error.empty()); + EXPECT_FALSE(runner.configured()); +} + +TEST(ControllerRunner, ConfigureFailsWhenTheInitialControllerCannotBeLoaded) +{ + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + + EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), "TestControllerMissing", error)); + EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình"; +} + +TEST(ControllerRunner, LoadsTheInitialControllerAndReportsItAsActive) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk"); + EXPECT_EQ(fixture.runner_.loadedCount(), 1U); +} + +TEST(ControllerRunner, SwapsBetweenControllersAndReusesLoadedLibraries) +{ + // swapPlanner chạy ở cửa vào mỗi yêu cầu (profile position/docking/...). Đổi qua lại không được + // dlopen lại. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary")); + EXPECT_EQ(fixture.runner_.loadedCount(), 2U); + + 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"; +} + +TEST(ControllerRunner, FailedSwapKeepsThePreviousControllerActive) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_FALSE(fixture.runner_.swapPlanner("TestControllerMissing")); + EXPECT_EQ(fixture.runner_.activeController(), "TestControllerOk"); +} + +TEST(ControllerRunner, SwapBeforeConfigureIsRefused) +{ + ControllerRunner runner; + EXPECT_FALSE(runner.swapPlanner("TestControllerOk")); +} + +// ================================================================================================ +// Trần vận tốc — đường tầng an toàn hạ tốc độ robot (bước 12) +// ================================================================================================ + +TEST(ControllerRunner, ForwardVelocityLimitReachesThePlugin) +{ + // Nếu lời gọi này không tới được plugin thì tầng an toàn yêu cầu giảm tốc mà robot vẫn chạy + // nguyên tốc độ planner — và không có dấu hiệu nào cả. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + robot_geometry_msgs::Twist cmd; + ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd)); + ASSERT_NEAR(cmd.linear.x, kBaseSpeed, 1e-9); + + 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"; +} + +TEST(ControllerRunner, AngularVelocityLimitReachesThePlugin) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + robot_geometry_msgs::Twist cmd; + ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd)); + ASSERT_NEAR(cmd.angular.z, kBaseYawRate, 1e-9); + + ASSERT_TRUE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.15))); // [rad/s] + ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd)); + EXPECT_NEAR(cmd.angular.z, 0.15, 1e-9); +} + +TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded) +{ + // Thứ tự khởi tạo không do move_base2 quyết: host có thể đặt trần trước khi controller được nạp. + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + + EXPECT_TRUE(runner.setTwistLinear(vec(0.08))); // [m/s], chưa có controller nào + ASSERT_TRUE(runner.swapPlanner("TestControllerOk")); + ASSERT_TRUE(runner.setPlan(makePlan())); + + 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"; +} + +TEST(ControllerRunner, LimitIsReappliedAfterSwappingController) +{ + // Trần thuộc về YÊU CẦU chứ không thuộc về instance planner. Instance mới không biết gì về trần + // đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.setTwistLinear(vec(0.07))); // [m/s] + ASSERT_TRUE(fixture.runner_.swapPlanner("TestControllerSecondary")); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + 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"; +} + +TEST(ControllerRunner, ControllerRefusingLimitsReportsFalse) +{ + // Host phải biết trần của nó không có hiệu lực, thay vì tưởng đã đặt được. + Fixture fixture("TestControllerRefusesLimits"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(0.10))); + EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, 0.10))); +} + +TEST(ControllerRunner, NonFiniteLimitIsRejected) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + const double nan = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(fixture.runner_.setTwistLinear(vec(nan))); + EXPECT_FALSE(fixture.runner_.setTwistAngular(vec(0.0, 0.0, nan))); +} + +// ================================================================================================ +// Vận tốc đo được +// ================================================================================================ + +TEST(ControllerRunner, MeasuredVelocityReachesThePlugin) +{ + // Interface gen-1 nhận vận tốc hiện tại làm tham số của computeVelocityCommands. Bản cũ đưa nó + // vào bằng con trỏ tới bộ nhớ host ghi (`setOdom(&odometry_)`) — một data race không có gì bảo + // vệ. Ở đây truyền theo giá trị. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + fixture.runner_.setMeasuredVelocity(twist(0.30)); // [m/s] + + 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"; +} + +TEST(ControllerRunner, NonFiniteMeasuredVelocityIsDroppedAndTheOldValueKept) +{ + // Nhiều local planner dùng vận tốc hiện tại làm mốc giới hạn gia tốc; NaN ở đó lan ra toàn bộ + // cost function. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + fixture.runner_.setMeasuredVelocity(twist(0.20)); + fixture.runner_.setMeasuredVelocity(twist(std::numeric_limits::quiet_NaN())); + + robot_geometry_msgs::Twist cmd; + ASSERT_TRUE(fixture.runner_.computeVelocityCommands(cmd)); + EXPECT_NEAR(cmd.linear.x, kBaseSpeed + 0.20, 1e-9); +} + +// ================================================================================================ +// Đường lỗi +// ================================================================================================ + +TEST(ControllerRunner, EmptyPlanIsRefused) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_FALSE(fixture.runner_.setPlan({})); +} + +TEST(ControllerRunner, SetPlanWithoutAControllerFails) +{ + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + + EXPECT_FALSE(runner.setPlan(makePlan())); +} + +TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand) +{ + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + + robot_geometry_msgs::Twist cmd; + EXPECT_FALSE(runner.computeVelocityCommands(cmd)); +} + +TEST(ControllerRunner, PluginReturningNoCommandIsPassedThroughAsFalse) +{ + Fixture fixture("TestControllerNoCommand"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + robot_geometry_msgs::Twist cmd; + EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); +} + +TEST(ControllerRunner, NaNCommandIsBlockedAtTheBoundary) +{ + // VelocityArbiter cũng chặn NaN, nhưng chặn 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. + Fixture fixture("TestControllerNaN"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + 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"; +} + +TEST(ControllerRunner, ExceptionFromThePluginIsContained) +{ + Fixture fixture("TestControllerThrowing"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + ASSERT_TRUE(fixture.runner_.setPlan(makePlan())); + + robot_geometry_msgs::Twist cmd; + EXPECT_NO_THROW({ EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); }); +} + +TEST(ControllerRunner, CommandIsClearedBeforeEveryAttempt) +{ + // Bên gọi dùng lại cùng một biến qua nhiều cycle. Trả false mà để nguyên lệnh cũ trong đó là mời + // tầng trên phát lại một lệnh đã hết hạn. + Fixture fixture("TestControllerNoCommand"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + robot_geometry_msgs::Twist cmd = twist(9.0, 9.0); + EXPECT_FALSE(fixture.runner_.computeVelocityCommands(cmd)); + EXPECT_NEAR(cmd.linear.x, 0.0, 1e-9); + EXPECT_NEAR(cmd.angular.z, 0.0, 1e-9); +} + +int main(int argc, char** argv) +{ + setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0); + setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0); + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/fake_ports.h b/test/fake_ports.h new file mode 100644 index 0000000..8f68dbd --- /dev/null +++ b/test/fake_ports.h @@ -0,0 +1,870 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực giả của các port, kịch bản hoá bằng chuỗi kết quả định sẵn. + * + * Đặt trong test/ của chính move_base2 chứ không đặt trong nav_test_harness: các fake này hiện thực + * port CỦA move_base2, nếu để trong harness thì harness phải phụ thuộc ngược vào move_base2 và + * chiều phụ thuộc một chiều bị phá vỡ. Phần fake thực sự dùng chung (đồng hồ, costmap, pose, + * kiểm va chạm, kịch bản) nằm ở nav_test_harness. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_TEST_FAKE_PORTS_H_ +#define MOVE_BASE2_TEST_FAKE_PORTS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace move_base2 +{ +namespace testing +{ + +/// @brief Kết quả một lần lập plan trong kịch bản. +enum class PlannerScript +{ + kOk, ///< Trả plan hợp lệ. + kFail, ///< makePlan trả false. + kEmpty ///< makePlan trả true nhưng plan rỗng — bẫy front()/back() trên vector rỗng. +}; + +/// @brief Kết quả một lần gọi controller trong kịch bản. +enum class ControllerScript +{ + kOk, ///< Sinh lệnh hợp lệ. + kFail, ///< Không sinh được lệnh. + kGoalReached, ///< Báo đã tới đích. + kNaN, ///< Sinh lệnh chứa NaN — phải bị bộ trọng tài chặn. + kTooFast ///< Sinh lệnh vượt trần vận tốc — phải bị clamp. +}; + +/// @brief Kết quả một tick recovery trong kịch bản. +enum class RecoveryScript +{ + kRunning, + kSucceeded, + kFailed +}; + +/// @brief Kết quả một tick action trong kịch bản (D8). +enum class ActionScript +{ + kRunning, + kSucceeded, + kFailed +}; + +// ------------------------------------------------------------------------------------------------ + +/// @brief Đồng hồ do test điều khiển, chuyển tiếp một FakeClock của harness qua ClockPort. +class FakeClockPort final : public ClockPort +{ +public: + explicit FakeClockPort(double start_sec = 1000.0) : now_(start_sec) + { + } + + robot::Time now() const override + { + return now_; + } + + /// @param seconds [s] Lượng thời gian trôi. Giá trị âm bị bỏ qua. + void advance(double seconds) + { + if (seconds > 0.0) + { + now_ = robot::Time(now_.toSec() + seconds); + } + } + + void setTime(double seconds) + { + now_ = robot::Time(seconds); + } + +private: + robot::Time now_; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakePosePort final : public PosePort +{ +public: + bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override + { + ++call_count_; + if (!available_) + { + return false; + } + pose = pose_; + return true; + } + + /// @param x,y [m] + void setPosition(double x, double y) + { + pose_.header.frame_id = "map"; + pose_.pose.position.x = x; + pose_.pose.position.y = y; + pose_.pose.orientation.w = 1.0; + } + + /// @brief false = mô phỏng TF thiếu/stale. + void setAvailable(bool available) + { + available_ = available; + } + + std::size_t callCount() const + { + return call_count_; + } + +private: + robot_geometry_msgs::PoseStamped pose_; + bool available_ = true; + mutable std::size_t call_count_ = 0; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakePlannerPort final : public PlannerPort +{ +public: + bool swapPlanner(const std::string& planner_name) override + { + if (!swap_succeeds_) + { + return false; + } + active_ = planner_name; + ++swap_count_; + return true; + } + + 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 + { + if (in_flight_) + { + return false; // Đúng như PlannerRunner: một lượt tại một thời điểm. + } + + ++make_plan_count_; + saw_order_ = saw_order_ || order != nullptr; + + in_flight_ = true; + pending_tag_ = tag; + pending_goal_ = goal; + cycles_left_ = latency_cycles_; + pending_action_ = nextAction(); + return true; + } + + bool isPlanning() const override + { + return in_flight_; + } + + bool pollPlan(PlanResult& result) override + { + if (!in_flight_) + { + return false; + } + if (cycles_left_ > 0) + { + --cycles_left_; + return false; // Còn "đang tính" — bên gọi phải thấy kBusy. + } + + in_flight_ = false; + result.tag = pending_tag_; + result.plan.clear(); + + switch (pending_action_) + { + case PlannerScript::kFail: + result.succeeded = false; + return true; + case PlannerScript::kEmpty: + // Contract nói thành công phải kèm plan không rỗng; fake cố ý vi phạm để kiểm guard của + // bên gọi. + result.succeeded = true; + return true; + case PlannerScript::kOk: + break; + } + + result.succeeded = true; + result.plan.push_back(pending_goal_); + return true; + } + + void cancelPlan() override + { + in_flight_ = false; + ++cancel_count_; + } + + std::string activePlanner() const override + { + return active_; + } + + /** + * @brief Số cycle mà một lượt lập plan "mất" trước khi có kết quả. + * + * 0 (mặc định) = kết quả có ngay ở lần poll kế tiếp, tức đúng nhịp của bản lập plan đồng bộ cũ: + * kick ở cuối cycle N, state machine thấy plan ở cycle N+1. Nhờ vậy mọi test viết cho bản đồng bộ + * giữ nguyên ý nghĩa. + */ + void setLatencyCycles(std::size_t cycles) + { + latency_cycles_ = cycles; + } + + std::size_t cancelCount() const + { + return cancel_count_; + } + + void setScript(std::vector script) + { + script_ = std::move(script); + index_ = 0; + } + + void setSwapSucceeds(bool succeeds) + { + swap_succeeds_ = succeeds; + } + + std::size_t makePlanCount() const + { + return make_plan_count_; + } + + std::size_t swapCount() const + { + return swap_count_; + } + + bool sawOrder() const + { + return saw_order_; + } + +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() + { + if (script_.empty()) + { + return PlannerScript::kOk; + } + if (index_ >= script_.size()) + { + return script_.back(); + } + return script_[index_++]; + } + + std::vector script_; + std::size_t index_ = 0; + std::string active_; + bool swap_succeeds_ = true; + + bool in_flight_ = false; + std::uint64_t pending_tag_ = 0; + robot_geometry_msgs::PoseStamped pending_goal_; + PlannerScript pending_action_ = PlannerScript::kOk; + std::size_t latency_cycles_ = 0; + std::size_t cycles_left_ = 0; + std::size_t cancel_count_ = 0; + + std::size_t make_plan_count_ = 0; + std::size_t swap_count_ = 0; + bool saw_order_ = false; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakeControllerPort final : public ControllerPort +{ +public: + bool swapPlanner(const std::string& planner_name) override + { + if (!swap_succeeds_) + { + return false; + } + active_ = planner_name; + return true; + } + + void setTolerance(double xy_m, double yaw_rad) override + { + xy_tolerance_ = xy_m; + yaw_tolerance_ = yaw_rad; + } + + bool setPlan(const std::vector& plan) override + { + ++set_plan_count_; + last_plan_size_ = plan.size(); + return set_plan_succeeds_ && !plan.empty(); + } + + bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override + { + ++compute_count_; + switch (current_action_) + { + case ControllerScript::kFail: + return false; + case ControllerScript::kNaN: + cmd.linear.x = std::numeric_limits::quiet_NaN(); + cmd.angular.z = 0.0; + return true; + case ControllerScript::kTooFast: + cmd.linear.x = 99.0; + cmd.angular.z = 99.0; + return true; + case ControllerScript::kGoalReached: + case ControllerScript::kOk: + break; + } + cmd.linear.x = nominal_speed_; + cmd.angular.z = 0.0; + return true; + } + + bool isGoalReached() override + { + // Lấy hành động cho cycle này ở đây vì đây là lời gọi ĐẦU TIÊN của một cycle controller, đúng + // thứ tự mà control loop dùng. + current_action_ = nextAction(); + ++goal_check_count_; + return current_action_ == ControllerScript::kGoalReached; + } + + void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override + { + measured_velocity_ = velocity; + } + + bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override + { + if (linear.x < 0.0) + { + limit_backward_ = linear.x; // [m/s], âm + } + else + { + limit_forward_ = linear.x; // [m/s] + } + return true; + } + + bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override + { + limit_angular_ = angular.z; // [rad/s] + return true; + } + + const robot_geometry_msgs::Twist& measuredVelocity() const + { + return measured_velocity_; + } + + double limitForward() const + { + return limit_forward_; + } + + double limitBackward() const + { + return limit_backward_; + } + + double limitAngular() const + { + return limit_angular_; + } + + std::string activeController() const override + { + return active_; + } + + void setScript(std::vector script) + { + script_ = std::move(script); + index_ = 0; + } + + void setSwapSucceeds(bool succeeds) + { + swap_succeeds_ = succeeds; + } + + void setSetPlanSucceeds(bool succeeds) + { + set_plan_succeeds_ = succeeds; + } + + /// @param speed [m/s] Tốc độ dài của lệnh khi kịch bản là kOk. + void setNominalSpeed(double speed) + { + nominal_speed_ = speed; + } + + std::size_t setPlanCount() const + { + return set_plan_count_; + } + + std::size_t computeCount() const + { + return compute_count_; + } + + std::size_t goalCheckCount() const + { + return goal_check_count_; + } + + std::size_t lastPlanSize() const + { + return last_plan_size_; + } + + double xyTolerance() const + { + return xy_tolerance_; + } + + double yawTolerance() const + { + return yaw_tolerance_; + } + +private: + robot_geometry_msgs::Twist measured_velocity_; + double limit_forward_ = 0.0; ///< [m/s] + double limit_backward_ = 0.0; ///< [m/s], âm + double limit_angular_ = 0.0; ///< [rad/s] + + ControllerScript nextAction() + { + if (script_.empty()) + { + return ControllerScript::kOk; + } + if (index_ >= script_.size()) + { + return script_.back(); + } + return script_[index_++]; + } + + std::vector script_; + std::size_t index_ = 0; + ControllerScript current_action_ = ControllerScript::kOk; + + std::string active_; + bool swap_succeeds_ = true; + bool set_plan_succeeds_ = true; + 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; + std::size_t goal_check_count_ = 0; + std::size_t last_plan_size_ = 0; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakeRecoveryPort final : public RecoveryPort +{ +public: + explicit FakeRecoveryPort(std::size_t behavior_count = 2) : behavior_count_(behavior_count) + { + } + + bool configure(robot::NodeHandle& /*nh*/) override + { + return true; + } + + std::size_t behaviorCount() const override + { + return behavior_count_; + } + + RecoveryOutputKind outputKind(std::size_t index) const override + { + if (index >= behavior_count_) + { + return RecoveryOutputKind::kNone; + } + const auto it = output_kinds_.find(index); + return it == output_kinds_.end() ? default_output_kind_ : it->second; + } + + /// @brief Đặt họ output cho behavior thứ @p index (mặc định mọi behavior đều lái robot). + void setOutputKind(std::size_t index, RecoveryOutputKind kind) + { + output_kinds_[index] = kind; + } + + void setDefaultOutputKind(RecoveryOutputKind kind) + { + default_output_kind_ = kind; + } + + bool start(std::size_t index, RecoveryTrigger trigger) override + { + ++start_count_; + last_start_index_ = index; + last_trigger_ = trigger; + started_indices_.push_back(index); + + if (index >= behavior_count_ || !start_succeeds_) + { + return false; + } + active_ = true; + return true; + } + + RecoveryTick update() override + { + ++update_count_; + + RecoveryTick tick; + switch (nextAction()) + { + case RecoveryScript::kRunning: + tick.status = RecoveryTick::Status::kRunning; + tick.has_velocity = emits_velocity_; + tick.cmd.linear.x = recovery_speed_; + break; + case RecoveryScript::kSucceeded: + tick.status = RecoveryTick::Status::kSucceeded; + active_ = false; + break; + case RecoveryScript::kFailed: + tick.status = RecoveryTick::Status::kFailed; + active_ = false; + break; + } + return tick; + } + + void cancel() override + { + ++cancel_count_; + active_ = false; + } + + std::string behaviorName(std::size_t index) const override + { + return index < behavior_count_ ? "fake_behavior_" + std::to_string(index) : std::string(); + } + + void setScript(std::vector script) + { + script_ = std::move(script); + index_ = 0; + } + + void setStartSucceeds(bool succeeds) + { + start_succeeds_ = succeeds; + } + + /// @param speed [m/s] Vận tốc behavior phát khi đang chạy. Dấu âm nghĩa là lùi. + void setRecoveryVelocity(bool emits, double speed) + { + emits_velocity_ = emits; + recovery_speed_ = speed; + } + + std::size_t startCount() const + { + return start_count_; + } + + std::size_t updateCount() const + { + return update_count_; + } + + std::size_t cancelCount() const + { + return cancel_count_; + } + + std::size_t lastStartIndex() const + { + return last_start_index_; + } + + RecoveryTrigger lastTrigger() const + { + return last_trigger_; + } + + const std::vector& startedIndices() const + { + return started_indices_; + } + + bool active() const + { + return active_; + } + +private: + RecoveryScript nextAction() + { + if (script_.empty()) + { + return RecoveryScript::kSucceeded; + } + if (index_ >= script_.size()) + { + return script_.back(); + } + return script_[index_++]; + } + + std::size_t behavior_count_; + std::vector script_; + std::size_t index_ = 0; + + bool start_succeeds_ = true; + bool active_ = false; + bool emits_velocity_ = false; + double recovery_speed_ = -0.1; ///< [m/s], âm = lùi + + /// Mặc định coi mọi behavior đều lái robot — giữ nguyên hành vi của các test viết trước khi + /// RecoveryPort có outputKind(). + RecoveryOutputKind default_output_kind_ = RecoveryOutputKind::kVelocity; + std::map output_kinds_; + + std::size_t start_count_ = 0; + std::size_t update_count_ = 0; + std::size_t cancel_count_ = 0; + std::size_t last_start_index_ = 0; + RecoveryTrigger last_trigger_ = RecoveryTrigger::kPlanningFailed; + std::vector started_indices_; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakeActionPort final : public ActionPort +{ +public: + bool configure(robot::NodeHandle& /*nh*/) override + { + return true; + } + + bool start(const robot_protocol_msgs::Action& action) override + { + ++start_count_; + started_action_types_.push_back(action.actionType); + if (!start_succeeds_) + { + return false; + } + active_ = true; + return true; + } + + ActionTick update() override + { + ++update_count_; + + ActionTick tick; + switch (nextAction()) + { + case ActionScript::kRunning: + tick.status = ActionTick::Status::kRunning; + break; + case ActionScript::kSucceeded: + tick.status = ActionTick::Status::kSucceeded; + active_ = false; + break; + case ActionScript::kFailed: + tick.status = ActionTick::Status::kFailed; + tick.message = "fake action failed"; + active_ = false; + break; + } + return tick; + } + + void cancel() override + { + ++cancel_count_; + active_ = false; + } + + void setScript(std::vector script) + { + script_ = std::move(script); + index_ = 0; + } + + void setStartSucceeds(bool succeeds) + { + start_succeeds_ = succeeds; + } + + std::size_t startCount() const + { + return start_count_; + } + + std::size_t updateCount() const + { + return update_count_; + } + + std::size_t cancelCount() const + { + return cancel_count_; + } + + /// @brief actionType của từng lần start, theo thứ tự — kiểm "actions đi nguyên vẹn, đúng thứ tự". + const std::vector& startedActionTypes() const + { + return started_action_types_; + } + + bool active() const + { + return active_; + } + +private: + ActionScript nextAction() + { + if (script_.empty()) + { + return ActionScript::kSucceeded; + } + if (index_ >= script_.size()) + { + return script_.back(); + } + return script_[index_++]; + } + + std::vector script_; + std::size_t index_ = 0; + + bool start_succeeds_ = true; + bool active_ = false; + + std::size_t start_count_ = 0; + std::size_t update_count_ = 0; + std::size_t cancel_count_ = 0; + std::vector started_action_types_; +}; + +// ------------------------------------------------------------------------------------------------ + +class FakeMissionPort final : public MissionPort +{ +public: + void setRequestCallback(RequestCallback callback) override + { + callback_ = std::move(callback); + } + + void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) override + { + reports_.emplace_back(mission_sequence_id, outcome); + } + + bool hasActiveMission() const override + { + return active_; + } + + void start() override + { + active_ = true; + } + + void stop() override + { + active_ = false; + } + + /// @brief Giả lập mission layer đẩy một chặng xuống. + void emit(const NavigationRequest& request) + { + if (callback_) + { + callback_(request); + } + } + + const std::vector>& reports() const + { + return reports_; + } + + /// @brief Số lần đã báo kết quả cho một sequence id — bất biến là phải bằng 1. + std::size_t reportCountFor(std::uint64_t mission_sequence_id) const + { + std::size_t count = 0; + for (const auto& report : reports_) + { + if (report.first == mission_sequence_id) + { + ++count; + } + } + return count; + } + +private: + RequestCallback callback_; + std::vector> reports_; + bool active_ = false; +}; + +} // namespace testing +} // namespace move_base2 + +#endif // MOVE_BASE2_TEST_FAKE_PORTS_H_ diff --git a/test/navigation_server_test.cpp b/test/navigation_server_test.cpp new file mode 100644 index 0000000..8a33f36 --- /dev/null +++ b/test/navigation_server_test.cpp @@ -0,0 +1,559 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test facade `NavigationServer`: đường lệnh vận tốc ra host, và đường dữ liệu cảm + * biến từ host vào costmap. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include "fake_ports.h" +#include "spy_layer.h" + +using move_base2::ControlLoopConfig; +using move_base2::ControlLoopDeps; +using move_base2::MotionProfile; +using move_base2::NavigationRequest; +using move_base2::NavigationServer; +using move_base2::NavigationState; +using move_base2::SensorGatewayConfig; +using move_base2::testing::attachSpy; +using move_base2::testing::ControllerScript; +using move_base2::testing::FakeActionPort; +using move_base2::testing::FakeClockPort; +using move_base2::testing::FakeControllerPort; +using move_base2::testing::FakeMissionPort; +using move_base2::testing::FakePlannerPort; +using move_base2::testing::FakePosePort; +using move_base2::testing::FakeRecoveryPort; +using move_base2::testing::PlannerScript; +using move_base2::testing::SpyPtr; +using robot_costmap_2d::LayerType; + +namespace +{ + +constexpr double kControlPeriod = 0.05; ///< [s] +constexpr double kClockStart = 1000.0; ///< [s] + +ControlLoopConfig baseConfig() +{ + ControlLoopConfig config; + + 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 = 2; + config.state_machine.recovery_enabled = true; + + config.velocity.max_vel_x = 0.5; // [m/s] + config.velocity.min_vel_x = -0.2; // [m/s] + config.velocity.max_vel_theta = 1.0; // [rad/s] + config.velocity.max_accel_x = 100.0; // [m/s^2] lớn để test không vướng ramp + config.velocity.max_accel_theta = 100.0; // [rad/s^2] + + config.nominal_control_period = kControlPeriod; + config.robot_base_frame = "base_link"; + + 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; + config.rotate = config.position; + + return config; +} + +NavigationRequest makeRequest(double goal_x) +{ + NavigationRequest request; + request.profile = MotionProfile::kPosition; + request.goal.header.frame_id = "map"; + request.goal.pose.position.x = goal_x; + request.goal.pose.orientation.w = 1.0; + return request; +} + +robot_geometry_msgs::Vector3 makeVector(double x, double y = 0.0, double z = 0.0) +{ + robot_geometry_msgs::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +robot_nav_msgs::Odometry makeOdometry(double vx, double wz) +{ + robot_nav_msgs::Odometry odom; + odom.header.frame_id = "odom"; + odom.twist.twist.linear.x = vx; // [m/s] + odom.twist.twist.angular.z = wz; // [rad/s] + return odom; +} + +robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F) +{ + robot_sensor_msgs::LaserScan scan; + scan.header.frame_id = "laser"; + scan.angle_min = -1.5F; // [rad] + scan.angle_max = 1.5F; // [rad] + scan.angle_increment = 3.0F / static_cast(rays); // [rad] + scan.range_min = 0.05F; // [m] + scan.range_max = 10.0F; // [m] + scan.ranges.assign(rays, range); + return scan; +} + +/** + * @class Fixture + * @brief `NavigationServer` nối đủ cổng giả, cộng hai costmap thật để kiểm đường cảm biến. + */ +class Fixture +{ +public: + Fixture() + : clock_(kClockStart) + , recovery_(2) + , global_("map", false, true) + , local_("odom", true, false) + { + pose_.setPosition(0.0, 0.0); + + deps_.clock = &clock_; + deps_.pose = &pose_; + deps_.planner = &planner_; + deps_.controller = &controller_; + deps_.recovery = &recovery_; + deps_.mission = &mission_; + deps_.action = &action_; + } + + void configure(const ControlLoopConfig& config = baseConfig()) + { + std::string error; + ASSERT_TRUE(server_.configureLoop(config, deps_, error)) << error; + } + + void configureSensors(const SensorGatewayConfig& config) + { + std::string error; + ASSERT_TRUE(server_.configureSensors(config, error)) << error; + } + + /// @brief Chạy @p cycles control cycle, mỗi cycle nhích đồng hồ giả một chu kỳ. + void spin(std::size_t cycles) + { + for (std::size_t i = 0; i < cycles; ++i) + { + server_.spinOnce(); + clock_.advance(kControlPeriod); + } + } + + void attachCostmaps() + { + server_.attachCostmaps(&global_, &local_); + } + + NavigationServer server_; + FakeClockPort clock_; + FakePosePort pose_; + FakePlannerPort planner_; + FakeControllerPort controller_; + FakeRecoveryPort recovery_; + FakeMissionPort mission_; + FakeActionPort action_; + ControlLoopDeps deps_; + + robot_costmap_2d::LayeredCostmap global_; + robot_costmap_2d::LayeredCostmap local_; +}; + +} // namespace + +// ================================================================================================ +// getTwist() — LỆNH vận tốc, không phải vận tốc đo được +// +// Host lấy getTwist() rồi publish thẳng ra /cmd_vel. Nếu giá trị đó đến từ odometry thì có một vòng +// lặp dương: robot chạy 0.5 m/s -> đọc odom 0.5 -> phát lệnh 0.5 -> mãi mãi. VelocityArbiter — toàn +// bộ hàng rào an toàn của gói — cũng bị bỏ qua hoàn toàn. Các test dưới đây khoá lại điều đó. +// ================================================================================================ + +TEST(NavigationServerTwist, ReturnsArbiterCommandNotOdometryVelocity) +{ + Fixture fixture; + fixture.configure(); + fixture.controller_.setNominalSpeed(0.3); // [m/s] + fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk }); + + // Odometry báo robot đang chạy nhanh hơn hẳn lệnh mà controller muốn phát. + fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9)); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + + fixture.spin(2); // IDLE -> PLANNING -> CONTROLLING (controller chạy ngay ở cycle này) + 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.theta, 0.0, 1e-9); +} + +TEST(NavigationServerTwist, OdometryAloneNeverProducesACommand) +{ + Fixture fixture; + fixture.configure(); + + fixture.server_.addOdometry("/odom", makeOdometry(1.7, 0.9)); + fixture.spin(1); // IDLE, không có yêu cầu nào + + const robot_nav_2d_msgs::Twist2DStamped twist = fixture.server_.getTwist(); + EXPECT_NEAR(twist.velocity.x, 0.0, 1e-9); + EXPECT_NEAR(twist.velocity.y, 0.0, 1e-9); + EXPECT_NEAR(twist.velocity.theta, 0.0, 1e-9); +} + +TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock) +{ + // Host loại lệnh quá hạn theo dấu này. Lấy giờ hệ thống lúc host hỏi sẽ làm một control loop đã + // treo vẫn trông như đang phát lệnh tươi — đúng thứ dấu thời gian sinh ra để ngăn. + Fixture fixture; + fixture.configure(); + + fixture.spin(1); + EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9); + + fixture.clock_.setTime(kClockStart + 12.0); + fixture.spin(1); + EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9); +} + +TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning) +{ + Fixture fixture; + fixture.configure(); + + fixture.spin(1); + const double stamp_after_first = fixture.server_.getTwist().header.stamp.toSec(); + + // Đồng hồ chạy tiếp nhưng KHÔNG có cycle nào — mô phỏng control thread treo. + fixture.clock_.setTime(kClockStart + 30.0); + 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"; +} + +TEST(NavigationServerTwist, IsStampedWithTheConfiguredRobotBaseFrame) +{ + ControlLoopConfig config = baseConfig(); + config.robot_base_frame = "base_footprint"; + + Fixture fixture; + fixture.configure(config); + fixture.spin(1); + + EXPECT_EQ(fixture.server_.getTwist().header.frame_id, "base_footprint"); +} + +TEST(NavigationServerTwist, ConfigureIsRefusedWhenRobotBaseFrameIsEmpty) +{ + ControlLoopConfig config = baseConfig(); + config.robot_base_frame.clear(); + + Fixture fixture; + std::string error; + EXPECT_FALSE(fixture.server_.configureLoop(config, fixture.deps_, error)); + EXPECT_FALSE(error.empty()); +} + +// ================================================================================================ +// Đường dữ liệu cảm biến từ host vào costmap +// ================================================================================================ + +TEST(NavigationServerSensors, SamplesReachTheCostmapLayersOnceAttached) +{ + Fixture fixture; + fixture.configure(); + + SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map"); + SpyPtr local_voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles"); + fixture.attachCostmaps(); + + fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + fixture.server_.addLaserScan("/b_scan", makeScan()); + 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->records()[0].topic, "/b_scan"); + EXPECT_EQ(local_voxel->records()[1].topic, "/camera/depth/points_proc"); +} + +TEST(NavigationServerSensors, StoringStillWorksWhenNoCostmapIsAttachedYet) +{ + // Trạng thái bình thường lúc khởi động: host đã bắt đầu bơm dữ liệu trước khi costmap được dựng. + // Dữ liệu vẫn phải đọc lại được qua getter của contract host, và số mẫu mất phải đếm được. + Fixture fixture; + fixture.configure(); + + fixture.server_.addLaserScan("/b_scan", makeScan()); + + EXPECT_EQ(fixture.server_.getLaserScan("/b_scan").ranges.size(), 40U); + EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U); + EXPECT_EQ(fixture.server_.sensors().stats().delivered, 0U); +} + +TEST(NavigationServerSensors, StaticMapReceivedBeforeAttachIsReplayed) +{ + // Không có phần phát lại này thì thứ tự "map tới trước, costmap dựng sau" — thứ tự thường gặp + // nhất khi khởi động — để global costmap trắng vĩnh viễn: /map là topic latched, host không gửi + // lại. Bản cũ bù bằng cặp biến public map_save_/map_name_save_. + Fixture fixture; + fixture.configure(); + + fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + ASSERT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 1U); + + 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"; + EXPECT_EQ(static_layer->records()[0].topic, "/map"); +} + +TEST(NavigationServerSensors, LegacyMapSavePublicMemberIsAlsoReplayed) +{ + // `map_save_`/`map_name_save_` là biến PUBLIC của BaseNavigation mà host tự gán + // (sensor_converter.cpp). Giữ đường này để host không phải sửa gì khi đổi sang move_base2. + Fixture fixture; + fixture.configure(); + + fixture.server_.map_name_save_ = "/map"; + fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid(); + + SpyPtr static_layer = attachSpy(fixture.global_, LayerType::STATIC_LAYER, "navigation_map"); + fixture.attachCostmaps(); + + EXPECT_EQ(static_layer->count(), 1U); + EXPECT_EQ(static_layer->records()[0].topic, "/map"); +} + +TEST(NavigationServerSensors, ReplayDoesNotDuplicateAMapAlreadyReceivedThroughTheApi) +{ + Fixture fixture; + fixture.configure(); + + fixture.server_.addStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + fixture.server_.map_name_save_ = "/map"; // host gán cả hai đường, như bản cũ đang làm + fixture.server_.map_save_ = robot_nav_msgs::OccupancyGrid(); + + 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"; +} + +TEST(NavigationServerSensors, StaleLaserScansAreNotReplayedOnAttach) +{ + // Cố ý: phát lại một scan cũ là dựng vật cản ở chỗ robot có thể đã rời khỏi từ lâu. Mẫu kế tiếp + // chỉ cách vài chục ms — chờ nó an toàn hơn hẳn. + Fixture fixture; + fixture.configure(); + + fixture.server_.addLaserScan("/b_scan", makeScan()); + + SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles"); + fixture.attachCostmaps(); + + EXPECT_EQ(voxel->count(), 0U); +} + +TEST(NavigationServerSensors, StoredLaserScanIsTheSameOneHandedToTheCostmap) +{ + // Bản cũ cất bản ĐÃ LỌC. Nếu getter trả bản thô còn costmap thấy bản lọc thì hai nguồn sự thật + // lệch nhau, và mọi chẩn đoán dựa trên getter sẽ nói dối về thứ costmap thật sự dùng. + SensorGatewayConfig sensors; + sensors.laser_sor_enabled = true; + sensors.laser_sor_mean_k = 5; + sensors.laser_sor_stddev_mul = 1.0; + + Fixture fixture; + fixture.configure(); + fixture.configureSensors(sensors); + + std::vector seen_by_layer; + SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles"); + voxel->setObserver([&seen_by_layer](const void* data, const std::type_info& type, + const std::string&) { + if (type == typeid(robot_sensor_msgs::LaserScan)) + { + seen_by_layer = static_cast(data)->ranges; + } + }); + fixture.attachCostmaps(); + + fixture.server_.addLaserScan("/b_scan", makeScan()); + + const std::vector stored = fixture.server_.getLaserScan("/b_scan").ranges; + ASSERT_FALSE(seen_by_layer.empty()); + ASSERT_EQ(stored.size(), seen_by_layer.size()); + + // So từng phần tử chứ không so cả vector: bộ lọc biến outlier thành NaN để giữ nguyên cấu trúc + // scan, mà NaN != NaN nên operator== của vector sẽ báo khác nhau dù nội dung giống hệt. + for (std::size_t i = 0; i < stored.size(); ++i) + { + if (std::isnan(stored[i])) + { + EXPECT_TRUE(std::isnan(seen_by_layer[i])) << "lệch tại tia " << i; + } + else + { + EXPECT_FLOAT_EQ(stored[i], seen_by_layer[i]) << "lệch tại tia " << i; + } + } +} + +TEST(NavigationServerSensors, DepthCameraDataIsStoredAndForwardedAsConstPtr) +{ + Fixture fixture; + fixture.configure(); + + SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles"); + fixture.attachCostmaps(); + + robot_sensor_msgs::DepthCameraData::Ptr data = + boost::make_shared(); + data->header.frame_id = "camera_optical"; + fixture.server_.addDepthCameraData("/camera/depth/data", data); + + ASSERT_EQ(voxel->count(), 1U); + EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr)); + EXPECT_EQ(voxel->records()[0].topic, "/camera/depth/data"); +} + +TEST(NavigationServerSensors, NullDepthPointerIsRejectedAtTheDoor) +{ + Fixture fixture; + fixture.configure(); + + SpyPtr voxel = attachSpy(fixture.local_, LayerType::VOXEL_LAYER, "obstacles"); + fixture.attachCostmaps(); + + fixture.server_.addDepthCameraData("/camera/depth/data", + robot_sensor_msgs::DepthCameraData::ConstPtr()); + + EXPECT_EQ(voxel->count(), 0U); + EXPECT_EQ(fixture.server_.sensors().stats().dropped_no_costmap, 0U); +} + +// ================================================================================================ +// Trần vận tốc (bước 12) — đường tầng an toàn hạ tốc độ robot +// +// `setTwistLinear` không phải lệnh jog dù tên nghe như vậy: host gọi nó theo cặp +v/-v để đặt trần +// cho hai chiều, và giá trị truyền xuống mang theo tốc độ đã bị tầng an toàn hạ +// (amr_control.cpp:561, 671-680). Trước đây `NavigationServer` trả false và không làm gì. +// ================================================================================================ + +TEST(NavigationServerLimits, ForwardAndBackwardLimitsReachTheController) +{ + Fixture fixture; + fixture.configure(); + + EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(0.30))); // [m/s] trần tiến + EXPECT_TRUE(fixture.server_.setTwistLinear(makeVector(-0.15))); // [m/s] trần lùi, ÂM + + fixture.spin(1); + + EXPECT_NEAR(fixture.controller_.limitForward(), 0.30, 1e-9); + EXPECT_NEAR(fixture.controller_.limitBackward(), -0.15, 1e-9); +} + +TEST(NavigationServerLimits, AngularLimitReachesTheController) +{ + Fixture fixture; + fixture.configure(); + + EXPECT_TRUE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, 0.45))); // [rad/s] + fixture.spin(1); + + EXPECT_NEAR(fixture.controller_.limitAngular(), 0.45, 1e-9); +} + +TEST(NavigationServerLimits, LimitTakesEffectInTheSameCycleItIsPushed) +{ + // Chậm một cycle nghĩa là một chu kỳ nữa robot chạy quá tốc độ mà tầng an toàn vừa yêu cầu hạ. + Fixture fixture; + fixture.configure(); + + ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.12))); + fixture.spin(1); + EXPECT_NEAR(fixture.controller_.limitForward(), 0.12, 1e-9); +} + +TEST(NavigationServerLimits, NonFiniteLimitIsRejectedAtTheDoor) +{ + Fixture fixture; + fixture.configure(); + + const double nan = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(fixture.server_.setTwistLinear(makeVector(nan))); + EXPECT_FALSE(fixture.server_.setTwistAngular(makeVector(0.0, 0.0, nan))); + + fixture.spin(1); + EXPECT_NEAR(fixture.controller_.limitForward(), 0.0, 1e-9); +} + +TEST(NavigationServerLimits, LatestLimitWinsWhenSetSeveralTimesWithinOneCycle) +{ + // Host gọi từ thread của nó với nhịp riêng; nhiều lời gọi giữa hai cycle là bình thường. Thứ phải + // có hiệu lực là giá trị MỚI NHẤT, không phải giá trị đầu tiên. + Fixture fixture; + fixture.configure(); + + ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.40))); + ASSERT_TRUE(fixture.server_.setTwistLinear(makeVector(0.10))); // tầng an toàn vừa hạ tiếp + fixture.spin(1); + + EXPECT_NEAR(fixture.controller_.limitForward(), 0.10, 1e-9); +} + +TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity) +{ + // Bản cũ đưa vận tốc đo được vào controller bằng con trỏ tới bộ nhớ host ghi + // (`tc_->setOdom(&odometry_)`) — data race không có gì bảo vệ. Ở đây truyền theo giá trị, qua + // control thread. + Fixture fixture; + fixture.configure(); + + fixture.server_.addOdometry("/odom", makeOdometry(0.42, -0.17)); + fixture.spin(1); + + EXPECT_NEAR(fixture.controller_.measuredVelocity().linear.x, 0.42, 1e-9); + EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9); +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/planner_runner_test.cpp b/test/planner_runner_test.cpp new file mode 100644 index 0000000..ef4585b --- /dev/null +++ b/test/planner_runner_test.cpp @@ -0,0 +1,422 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test PlannerRunner: nạp plugin thật qua Boost.DLL, và mọi đường lỗi phải trả false + * chứ không được để dữ liệu hỏng đi tiếp. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace +{ +using move_base2::PlannerRunner; + +/** + * @brief Con trỏ costmap giả. + * + * `PlannerRunner::configure` từ chối costmap null — đúng, vì mọi plugin thật đều dùng nó. Nhưng + * `Costmap2DROBOT` không dựng được trong unit test (cần `tf3::BufferCore` thật và cây config đầy + * đủ), nên test dùng một địa chỉ hợp lệ nhưng không phải costmap. + * + * An toàn ở đây vì `test_global_planner.cpp` **không alias nào chạm vào con trỏ này** — nó chỉ được + * chuyển tiếp qua `initialize()` rồi bị bỏ qua. Đường có costmap thật thuộc test tích hợp (Phase 5). + */ +robot_costmap_2d::Costmap2DROBOT* dummyCostmap() +{ + static std::uintptr_t placeholder = 0; + return reinterpret_cast(&placeholder); +} + +/// @brief Chạy trọn một lượt lập plan đồng bộ hoá lại cho test: kick, chờ thread, lấy kết quả. +bool runOnePlan(move_base2::PlannerRunner& runner, const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + const robot_protocol_msgs::Order* order, move_base2::PlanResult& result) +{ + if (!runner.startPlan(start, goal, order, /*tag=*/1)) + { + return false; + } + // Thread planner là thread thật; test phải chờ nó. Vòng quay ngắn thay vì sleep cố định để test + // không phụ thuộc vào tốc độ máy. + for (int i = 0; i < 10000 && runner.isPlanning(); ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + return runner.pollPlan(result); +} + +robot_geometry_msgs::PoseStamped makePose(double x, double y) +{ + robot_geometry_msgs::PoseStamped pose; + pose.header.frame_id = "map"; + pose.pose.position.x = x; // [m] + pose.pose.position.y = y; // [m] + pose.pose.orientation.w = 1.0; + return pose; +} + +/// @brief Runner đã configure với planner @p name; ASSERT nếu không nạp được. +class Fixture +{ +public: + explicit Fixture(const std::string& name = "TestPlannerOk") + { + robot::NodeHandle nh; + std::string error; + ok_ = runner_.configure(nh, dummyCostmap(), name, error); + error_ = error; + } + + bool ok() const + { + return ok_; + } + + const std::string& error() const + { + return error_; + } + + PlannerRunner runner_; + +private: + bool ok_ = false; + std::string error_; +}; + +} // namespace + +// ================================================================================================ +// Cấu hình +// ================================================================================================ + +TEST(PlannerRunner, RefusesNullCostmap) +{ + // Plugin thật nào cũng dùng costmap. Nhận null rồi chuyển tiếp xuống `initialize()` là đẩy quyết + // định "sập hay không" cho từng plugin tự lo. + robot::NodeHandle nh; + PlannerRunner runner; + std::string error; + + EXPECT_FALSE(runner.configure(nh, nullptr, "TestPlannerOk", error)); + EXPECT_FALSE(error.empty()); + EXPECT_FALSE(runner.configured()); +} + +TEST(PlannerRunner, ConfiguresWithoutAnInitialPlanner) +{ + robot::NodeHandle nh; + PlannerRunner runner; + std::string error; + + ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error; + EXPECT_TRUE(runner.configured()); + EXPECT_TRUE(runner.activePlanner().empty()); + EXPECT_EQ(runner.loadedCount(), 0U); +} + +TEST(PlannerRunner, RefusesSecondConfigure) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + robot::NodeHandle nh; + std::string error; + EXPECT_FALSE(fixture.runner_.configure(nh, dummyCostmap(), "TestPlannerOk", error)); +} + +TEST(PlannerRunner, ConfigureFailsWhenTheInitialPlannerCannotBeLoaded) +{ + robot::NodeHandle nh; + PlannerRunner runner; + std::string error; + + 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"; +} + +// ================================================================================================ +// Nạp plugin qua Boost.DLL +// ================================================================================================ + +TEST(PlannerRunner, LoadsTheInitialPlannerAndReportsItAsActive) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk"); + EXPECT_EQ(fixture.runner_.loadedCount(), 1U); +} + +TEST(PlannerRunner, SwapsBetweenPlannersAndReusesLoadedLibraries) +{ + // swapPlanner chạy ở CỬA VÀO mỗi yêu cầu. Đổi qua lại giữa hai profile không được dlopen lại. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.swapPlanner("TestPlannerEmptyPlan")); + EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerEmptyPlan"); + EXPECT_EQ(fixture.runner_.loadedCount(), 2U); + + 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"; +} + +TEST(PlannerRunner, FailedSwapKeepsThePreviousPlannerActive) +{ + // Bên gọi từ chối yêu cầu dựa trên giá trị trả về. Chuyển sang trạng thái "không có planner" sẽ + // giết luôn yêu cầu đang chạy dở, dù nó chẳng liên quan gì tới planner vừa nạp hỏng. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_FALSE(fixture.runner_.swapPlanner("TestPlannerMissingLibrary")); + EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk"); + + move_base2::PlanResult result; + ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result)); + EXPECT_TRUE(result.succeeded); +} + +TEST(PlannerRunner, PlannerReportingInitializeFailureIsRejected) +{ + // Bản cũ chỉ log rồi đi tiếp với planner chưa khởi tạo xong. + robot::NodeHandle nh; + PlannerRunner runner; + std::string error; + + EXPECT_FALSE(runner.configure(nh, dummyCostmap(), "TestPlannerInitFails", error)); +} + +TEST(PlannerRunner, PlannerThatFailedToInitializeIsNotCached) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + 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 đó"; +} + +TEST(PlannerRunner, RefusesEmptyPlannerName) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + EXPECT_FALSE(fixture.runner_.swapPlanner("")); + EXPECT_EQ(fixture.runner_.activePlanner(), "TestPlannerOk"); +} + +TEST(PlannerRunner, SwapBeforeConfigureIsRefused) +{ + PlannerRunner runner; + EXPECT_FALSE(runner.swapPlanner("TestPlannerOk")); +} + +// ================================================================================================ +// Một lượt lập plan — mọi đường lỗi phải báo thất bại, plan phải rỗng +// ================================================================================================ + +TEST(PlannerRunner, ProducesANonEmptyPlanOnTheHappyPath) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + move_base2::PlanResult result; + ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 1.0), nullptr, result)); + ASSERT_TRUE(result.succeeded); + ASSERT_FALSE(result.plan.empty()); + EXPECT_EQ(result.tag, 1U); + EXPECT_DOUBLE_EQ(result.plan.back().pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(result.plan.back().pose.position.y, 1.0); +} + +TEST(PlannerRunner, OrderIsForwardedToTheOrderAwareOverload) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + const robot_protocol_msgs::Order order; + move_base2::PlanResult result; + ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), &order, result)); + EXPECT_TRUE(result.succeeded); + EXPECT_FALSE(result.plan.empty()); +} + +TEST(PlannerRunner, OrderIsCopiedSoItMayDieBeforeThePlanFinishes) +{ + // Con trỏ Order chỉ hợp lệ trong lời gọi startPlan, nhưng lượt lập plan sống lâu hơn thế. Không + // sao chép là thread planner đọc bộ nhớ đã chết. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + { + const robot_protocol_msgs::Order order; + ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), &order, 7)); + } // order chết ở đây + + for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + move_base2::PlanResult result; + ASSERT_TRUE(fixture.runner_.pollPlan(result)); + EXPECT_TRUE(result.succeeded); + EXPECT_EQ(result.tag, 7U); +} + +TEST(PlannerRunner, PlannerReturningTrueWithAnEmptyPlanIsTreatedAsFailure) +{ + // Contract của PlannerPort: thành công nghĩa là plan KHÔNG rỗng. Lọt qua thì tầng trên gọi + // front()/back() trên vector rỗng. + Fixture fixture("TestPlannerEmptyPlan"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + move_base2::PlanResult result; + ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result)); + EXPECT_FALSE(result.succeeded); + EXPECT_TRUE(result.plan.empty()); +} + +TEST(PlannerRunner, ExceptionFromThePluginIsContained) +{ + // Plugin là code bên thứ ba nạp lúc chạy. Exception thoát khỏi thân thread là std::terminate — + // mất cả tiến trình navigation vì một lượt lập plan hỏng. + Fixture fixture("TestPlannerThrowing"); + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + move_base2::PlanResult result; + ASSERT_TRUE(runOnePlan(fixture.runner_, makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, result)); + EXPECT_FALSE(result.succeeded); + EXPECT_TRUE(result.plan.empty()); +} + +TEST(PlannerRunner, NonFiniteStartOrGoalIsRejectedBeforeStartingTheThread) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + const double nan = std::numeric_limits::quiet_NaN(); + const double inf = std::numeric_limits::infinity(); + + EXPECT_FALSE(fixture.runner_.startPlan(makePose(nan, 0.0), makePose(2.0, 0.0), nullptr, 1)); + EXPECT_FALSE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(inf, 0.0), nullptr, 1)); + EXPECT_FALSE(fixture.runner_.isPlanning()); +} + +TEST(PlannerRunner, StartPlanWithoutAnActivePlannerFails) +{ + robot::NodeHandle nh; + PlannerRunner runner; + std::string error; + ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "", error)) << error; + + EXPECT_FALSE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1)); +} + +TEST(PlannerRunner, SecondStartWhileOneIsInFlightIsRefused) +{ + // Một lượt tại một thời điểm. Nhận thêm sẽ đè lên yêu cầu đang chạy và làm mất công đã bỏ ra. + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1)); + const bool refused = !fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(3.0, 0.0), nullptr, 2); + + for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // Lượt đầu có thể đã xong trước lời gọi thứ hai (planner giả rất nhanh), nên chỉ khẳng định điều + // luôn đúng: không bao giờ có hai lượt cùng chạy, và kết quả thu về là của MỘT lượt. + 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ư"; +} + +TEST(PlannerRunner, CancelledPlanProducesNoResult) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1)); + fixture.runner_.cancelPlan(); + + for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + 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"; +} + +TEST(PlannerRunner, ResultCarriesBackTheTagItWasStartedWith) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + ASSERT_TRUE(fixture.runner_.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 42)); + for (int i = 0; i < 10000 && fixture.runner_.isPlanning(); ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + move_base2::PlanResult result; + ASSERT_TRUE(fixture.runner_.pollPlan(result)); + EXPECT_EQ(result.tag, 42U); +} + +TEST(PlannerRunner, PollOnAnIdleRunnerReturnsNothing) +{ + Fixture fixture; + ASSERT_TRUE(fixture.ok()) << fixture.error(); + + move_base2::PlanResult result; + EXPECT_FALSE(fixture.runner_.pollPlan(result)); + EXPECT_FALSE(fixture.runner_.isPlanning()); +} + +TEST(PlannerRunner, DestructorJoinsWhileAPlanIsInFlight) +{ + // Detach thay vì join sẽ để thread chạm vào buffer đã bị huỷ. Test này chạy sạch dưới sanitizer + // là bằng chứng; ở đây nó ít nhất khẳng định destructor không treo. + robot::NodeHandle nh; + std::string error; + { + PlannerRunner runner; + ASSERT_TRUE(runner.configure(nh, dummyCostmap(), "TestPlannerOk", error)) << error; + EXPECT_TRUE(runner.startPlan(makePose(0.0, 0.0), makePose(2.0, 0.0), nullptr, 1)); + } + SUCCEED(); +} + +int main(int argc, char** argv) +{ + // ctest không mang theo biến môi trường của shell; binary tự trỏ vào cây config và thư viện của + // gói, đúng cách recovery_runner_test và action_runner_test đang làm. + setenv("PNKX_NAV_CORE_CONFIG_DIR", MOVE_BASE2_TEST_CONFIG_DIR, 0); + setenv("PNKX_NAV_CORE_LIBRARY_PATH", MOVE_BASE2_TEST_LIBRARY_DIR, 0); + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/plugins/test_global_planner.cpp b/test/plugins/test_global_planner.cpp new file mode 100644 index 0000000..8379a61 --- /dev/null +++ b/test/plugins/test_global_planner.cpp @@ -0,0 +1,128 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — plugin global planner CHỈ dùng cho test. + * + * Bốn alias được export từ cùng một thư viện, mỗi alias là một hành vi mà `PlannerRunner` phải xử lý + * đúng. Chọn cách này thay vì một plugin đọc config: nó làm test không phụ thuộc vào cây config, và + * mỗi kịch bản gọi tên đúng thứ nó kiểm. + * + * Không alias nào chạm vào con trỏ costmap — đó là điều kiện để test truyền vào một con trỏ giả + * thay vì phải dựng `Costmap2DROBOT` thật (cần tf3::BufferCore và cây config đầy đủ). + * + * Author: DuongTD + *********************************************************************/ +#include +#include +#include + +#include +#include +#include + +#include + +namespace move_base2 +{ +namespace testing +{ + +/// @brief Số pose trong plan mà `TestPlannerOk` sinh ra. +constexpr std::size_t kPlanLength = 3; + +/** + * @class TestGlobalPlanner + * @brief Planner giả, hành vi cố định theo tham số dựng. + */ +class TestGlobalPlanner : public robot_nav_core::BaseGlobalPlanner +{ +public: + enum class Behavior + { + kOk, ///< Trả plan hợp lệ. + kEmptyPlan, ///< Trả true kèm plan RỖNG — bẫy mà PlannerRunner phải quy về false. + kThrow, ///< Ném exception giữa lúc lập plan. + kInitFails ///< initialize() trả false. + }; + + explicit TestGlobalPlanner(Behavior behavior) : behavior_(behavior) + { + } + + bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override + { + // Cố ý KHÔNG chạm costmap_robot — xem chú thích đầu file. + name_ = std::move(name); + return behavior_ != Behavior::kInitFails; + } + + bool makePlan(const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& plan) override + { + ++call_count_; + + if (behavior_ == Behavior::kThrow) + { + throw std::runtime_error("TestGlobalPlanner được yêu cầu ném exception"); + } + + plan.clear(); + if (behavior_ == Behavior::kEmptyPlan) + { + return true; // true + rỗng: đúng thứ contract PlannerPort cấm lọt qua. + } + + plan.push_back(start); + for (std::size_t i = plan.size(); i + 1 < kPlanLength; ++i) + { + plan.push_back(start); + } + plan.push_back(goal); + return true; + } + + bool makePlan(const robot_protocol_msgs::Order& /*msg*/, + const robot_geometry_msgs::PoseStamped& start, + const robot_geometry_msgs::PoseStamped& goal, + std::vector& plan) override + { + saw_order_ = true; + return makePlan(start, goal, plan); + } + +private: + Behavior behavior_; + std::string name_; + std::size_t call_count_ = 0; + bool saw_order_ = false; +}; + +robot_nav_core::BaseGlobalPlanner::Ptr createOk() +{ + return std::make_shared(TestGlobalPlanner::Behavior::kOk); +} + +robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan() +{ + return std::make_shared(TestGlobalPlanner::Behavior::kEmptyPlan); +} + +robot_nav_core::BaseGlobalPlanner::Ptr createThrowing() +{ + return std::make_shared(TestGlobalPlanner::Behavior::kThrow); +} + +robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing() +{ + return std::make_shared(TestGlobalPlanner::Behavior::kInitFails); +} + +} // namespace testing +} // namespace move_base2 + +BOOST_DLL_ALIAS(move_base2::testing::createOk, TestPlannerOk) +BOOST_DLL_ALIAS(move_base2::testing::createEmptyPlan, TestPlannerEmptyPlan) +BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestPlannerThrowing) +BOOST_DLL_ALIAS(move_base2::testing::createInitFailing, TestPlannerInitFails) diff --git a/test/plugins/test_local_planner.cpp b/test/plugins/test_local_planner.cpp new file mode 100644 index 0000000..c3d8769 --- /dev/null +++ b/test/plugins/test_local_planner.cpp @@ -0,0 +1,216 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — plugin local planner CHỈ dùng cho test. + * + * Instance được nạp qua Boost.DLL nên test không giữ được con trỏ tới nó. Thay vì mở một cửa hậu để + * đọc trạng thái, planner này **phản ánh** thứ nó nhận được vào chính lệnh vận tốc nó trả về: + * + * cmd.linear.x = clamp(kBaseSpeed + vận_tốc_đo_được.x, trần_tiến) + * cmd.angular.z = clamp(kBaseYawRate, trần_góc) + * + * Nhờ vậy "trần đã tới plugin chưa" và "vận tốc đo được đã tới plugin chưa" kiểm được qua đúng API + * mà runtime dùng, không cần cơ chế quan sát riêng nào. + * + * Không alias nào chạm vào con trỏ TF hay costmap — đó là điều kiện để test truyền con trỏ giả thay + * vì phải dựng `tf3::BufferCore` và `Costmap2DROBOT` thật. + * + * Author: DuongTD + *********************************************************************/ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace move_base2 +{ +namespace testing +{ + +constexpr double kBaseSpeed = 0.25; ///< [m/s] lệnh nền khi chưa có trần nào +constexpr double kBaseYawRate = 0.40; ///< [rad/s] + +/** + * @class TestLocalPlanner + * @brief Local planner giả, hành vi cố định theo tham số dựng. + */ +class TestLocalPlanner : public robot_nav_core::BaseLocalPlanner +{ +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). + }; + + explicit TestLocalPlanner(Behavior behavior) : behavior_(behavior) + { + } + + void initialize(std::string name, tf3::BufferCore* /*tf*/, + robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override + { + // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file. + name_ = std::move(name); + } + + bool setPlan(const std::vector& plan) override + { + return !plan.empty(); + } + + void getPlan(std::vector& path) override + { + path.clear(); + } + + void getGlobalPlan(std::vector& path) override + { + path.clear(); + } + + bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity, + robot_geometry_msgs::Twist& cmd_vel) override + { + switch (behavior_) + { + case Behavior::kThrow: + throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception"); + case Behavior::kNoCommand: + return false; + case Behavior::kNaN: + cmd_vel.linear.x = std::numeric_limits::quiet_NaN(); + return true; + case Behavior::kOk: + case Behavior::kRefusesLimits: + break; + } + + double linear = kBaseSpeed + velocity.linear.x; + if (has_limit_forward_) + { + linear = std::min(linear, limit_forward_); + } + + double yaw = kBaseYawRate; + if (has_limit_angular_) + { + yaw = std::min(yaw, limit_angular_); + } + + cmd_vel.linear.x = linear; + cmd_vel.angular.z = yaw; + return true; + } + + bool isGoalReached() override + { + return false; + } + + bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override + { + if (behavior_ == Behavior::kRefusesLimits) + { + return false; + } + // Dấu chọn chiều, đúng quy ước của interface gen-1. + if (linear.x < 0.0) + { + limit_backward_ = linear.x; + } + else + { + limit_forward_ = linear.x; + has_limit_forward_ = true; + } + return true; + } + + robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override + { + robot_geometry_msgs::Vector3 out; + out.x = direct ? limit_forward_ : limit_backward_; + return out; + } + + bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override + { + if (behavior_ == Behavior::kRefusesLimits) + { + return false; + } + limit_angular_ = angular.z; + has_limit_angular_ = true; + return true; + } + + robot_geometry_msgs::Vector3 getTwistAngular(bool /*direct*/) override + { + robot_geometry_msgs::Vector3 out; + out.z = limit_angular_; + return out; + } + +private: + Behavior behavior_; + std::string name_; + double limit_forward_ = 0.0; ///< [m/s] + double limit_backward_ = 0.0; ///< [m/s], âm + double limit_angular_ = 0.0; ///< [rad/s] + bool has_limit_forward_ = false; + bool has_limit_angular_ = false; +}; + +robot_nav_core::BaseLocalPlanner::Ptr createOk() +{ + return std::make_shared(TestLocalPlanner::Behavior::kOk); +} + +robot_nav_core::BaseLocalPlanner::Ptr createSecondary() +{ + return std::make_shared(TestLocalPlanner::Behavior::kOk); +} + +robot_nav_core::BaseLocalPlanner::Ptr createNoCommand() +{ + return std::make_shared(TestLocalPlanner::Behavior::kNoCommand); +} + +robot_nav_core::BaseLocalPlanner::Ptr createNaN() +{ + return std::make_shared(TestLocalPlanner::Behavior::kNaN); +} + +robot_nav_core::BaseLocalPlanner::Ptr createThrowing() +{ + return std::make_shared(TestLocalPlanner::Behavior::kThrow); +} + +robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits() +{ + return std::make_shared(TestLocalPlanner::Behavior::kRefusesLimits); +} + +} // namespace testing +} // namespace move_base2 + +BOOST_DLL_ALIAS(move_base2::testing::createOk, TestControllerOk) +BOOST_DLL_ALIAS(move_base2::testing::createSecondary, TestControllerSecondary) +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) diff --git a/test/recovery_runner_test.cpp b/test/recovery_runner_test.cpp new file mode 100644 index 0000000..26f0ce8 --- /dev/null +++ b/test/recovery_runner_test.cpp @@ -0,0 +1,237 @@ +/********************************************************************* + * + * move_base2 — kiểm chỗ nối RecoveryPort <-> recovery_core. + * + * Đây là seam giữa hai gói: nếu nó đúng thì mọi behavior của recovery_core dùng được từ lõi mà lõi + * không biết gì về recovery_core. Test nạp plugin qua đúng đường Boost.DLL mà runtime đi. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include + +#include + +#include + +#include "fake_ports.h" + +namespace +{ +using move_base2::RecoveryOutputKind; +using move_base2::RecoveryRunner; +using move_base2::RecoveryTick; +using move_base2::RecoveryTrigger; +using move_base2::testing::FakeClockPort; +using move_base2::testing::FakePosePort; + +/// Bộ đồ nghề tối thiểu: đồng hồ giả + pose giả, không costmap (behavior họ kNone không cần). +struct Rig +{ + Rig() + { + pose.setPosition(0.0, 0.0); + + RecoveryRunner::Deps deps; + deps.clock = &clock; + deps.pose = &pose; + runner.setDeps(deps); + } + + bool load(const std::string& ns) + { + runner.setNamespace(ns); + robot::NodeHandle nh; + return runner.configure(nh); + } + + FakeClockPort clock{1000.0}; + FakePosePort pose; + RecoveryRunner runner; +}; + +TEST(RecoveryRunner, LoadsBehaviorsInDeclaredOrder) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + ASSERT_EQ(rig.runner.behaviorCount(), 2u); + EXPECT_EQ(rig.runner.behaviorName(0), "wait_short"); + EXPECT_EQ(rig.runner.behaviorName(1), "wait_long"); +} + +TEST(RecoveryRunner, ReportsOutputKindOfLoadedBehaviors) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + EXPECT_EQ(rig.runner.outputKind(0), RecoveryOutputKind::kNone); + EXPECT_EQ(rig.runner.outputKind(1), RecoveryOutputKind::kNone); +} + +TEST(RecoveryRunner, OutOfRangeIndexIsSafeAndNeverClaimsVelocity) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + // Giả định an toàn: không biết là gì thì không cấp quyền phát vận tốc. + EXPECT_EQ(rig.runner.outputKind(99), RecoveryOutputKind::kNone); + EXPECT_TRUE(rig.runner.behaviorName(99).empty()); + EXPECT_FALSE(rig.runner.start(99, RecoveryTrigger::kPlanningFailed)); +} + +TEST(RecoveryRunner, StartBeforeConfigureFails) +{ + Rig rig; + EXPECT_FALSE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed)); +} + +TEST(RecoveryRunner, UpdateWithoutActiveBehaviorFailsInsteadOfCrashing) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + const RecoveryTick tick = rig.runner.update(); + + EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed); + EXPECT_FALSE(tick.has_velocity); + EXPECT_FALSE(tick.message.empty()); +} + +TEST(RecoveryRunner, RunsBehaviorToSuccessOnRealClock) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed)); // wait_duration: 1.0 s + + rig.clock.advance(0.5); + EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning); + + rig.clock.advance(0.5); + EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded); +} + +TEST(RecoveryRunner, NoneFamilyNeverReportsVelocityToTheCore) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed)); + + for (int i = 0; i < 5; ++i) + { + rig.clock.advance(0.3); + const RecoveryTick tick = rig.runner.update(); + // Lõi dùng has_velocity để quyết định có lấy cmd hay không; behavior đứng yên không được bật. + EXPECT_FALSE(tick.has_velocity); + EXPECT_FALSE(tick.has_path); + if (tick.status != RecoveryTick::Status::kRunning) + { + break; + } + } +} + +TEST(RecoveryRunner, BehaviorTimeoutSurfacesAsFailed) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + // wait_long: wait_duration 5 s nhưng timeout 3 s -> phải kết thúc bằng kFailed, không treo. + ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kControllingFailed)); + + rig.clock.advance(2.0); + ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning); + + rig.clock.advance(1.5); + const RecoveryTick tick = rig.runner.update(); + + EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed); + EXPECT_NE(tick.message.find("timeout"), std::string::npos); +} + +TEST(RecoveryRunner, CancelIsSafeWithoutActiveBehavior) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + rig.runner.cancel(); // không được crash + SUCCEED(); +} + +TEST(RecoveryRunner, CancelledTickSurfacesAsFailedNotRunning) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed)); + + rig.runner.cancel(); + rig.clock.advance(0.1); + const RecoveryTick tick = rig.runner.update(); + + // State machine hiện tại không tick sau cancel, nên nhánh này không đạt tới trong runtime thật. + // Nhưng nếu ai đó nới điều kiện tick, kCancelled phải thành kFailed chứ không im lặng thành + // kRunning — đó là lý do nhánh dịch được giữ lại. + EXPECT_EQ(tick.status, RecoveryTick::Status::kFailed); +} + +TEST(RecoveryRunner, RestartingSecondBehaviorWorks) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + ASSERT_TRUE(rig.runner.start(0, RecoveryTrigger::kPlanningFailed)); + rig.clock.advance(1.0); + ASSERT_EQ(rig.runner.update().status, RecoveryTick::Status::kSucceeded); + + // State machine chuyển sang behavior kế tiếp sau khi cái trước kết thúc. + ASSERT_TRUE(rig.runner.start(1, RecoveryTrigger::kOscillation)); + rig.clock.advance(0.5); + EXPECT_EQ(rig.runner.update().status, RecoveryTick::Status::kRunning); +} + +TEST(RecoveryRunner, EmptyBehaviorListFailsConfigure) +{ + Rig rig; + EXPECT_FALSE(rig.load("recovery_empty")); + EXPECT_EQ(rig.runner.behaviorCount(), 0u); +} + +TEST(RecoveryRunner, MissingLibraryPathFailsConfigure) +{ + Rig rig; + EXPECT_FALSE(rig.load("recovery_missing_library")); + EXPECT_EQ(rig.runner.behaviorCount(), 0u); +} + +TEST(RecoveryRunner, ConfigureRequiresClockAndPose) +{ + RecoveryRunner runner; + 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"; +} + +TEST(RecoveryRunner, ConfigureTwiceRejected) +{ + Rig rig; + ASSERT_TRUE(rig.load("recovery")); + + robot::NodeHandle nh; + EXPECT_FALSE(rig.runner.configure(nh)); +} + +} // namespace + +int main(int argc, char** argv) +{ +#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/sensor_gateway_test.cpp b/test/sensor_gateway_test.cpp new file mode 100644 index 0000000..4363bed --- /dev/null +++ b/test/sensor_gateway_test.cpp @@ -0,0 +1,393 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test đường vào cảm biến. + * + * Test này dựng `LayeredCostmap` THẬT và cắm vào đó các layer gián điệp. Lý do không dùng costmap + * giả: thứ cần khoá lại ở đây chính là ba contract ẩn của `robot_costmap_2d`, và chúng chỉ tồn tại + * trong lớp thật — + * 1. `Layer::dataCallBack` xoá kiểu về `void*` + `std::type_info`, sai kiểu KHÔNG gây lỗi biên + * dịch mà rơi im lặng; + * 2. tham số `name` là khoá topic mà layer so lại, không phải nhãn tự do; + * 3. bộ lọc chọn layer quyết định layer nào thấy dữ liệu. + * Một costmap giả sẽ mô phỏng lại các contract đó theo cách tôi *nghĩ* chúng hoạt động — đúng loại + * test không phát hiện được gì. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include "spy_layer.h" + +using move_base2::SensorGateway; +using move_base2::SensorGatewayConfig; +using move_base2::testing::attachSpy; +using move_base2::testing::SpyPtr; +using robot_costmap_2d::LayerType; + +namespace +{ + +/** + * @class Bench + * @brief Một cặp costmap thật + cổng cảm biến đã gắn. + */ +class Bench +{ +public: + Bench() + : global_("map", false, true) + , local_("odom", true, false) + { + std::string error; + EXPECT_TRUE(gateway_.configure(SensorGatewayConfig(), error)) << error; + } + + /// @brief Cắm một layer gián điệp vào costmap global và trả con trỏ để kiểm tra sau. + SpyPtr addGlobal(LayerType type, const std::string& name, bool enabled = true, bool explode = false) + { + return attachSpy(global_, type, name, enabled, explode); + } + + SpyPtr addLocal(LayerType type, const std::string& name, bool enabled = true, bool explode = false) + { + return attachSpy(local_, type, name, enabled, explode); + } + + void attach() + { + gateway_.attach(&global_, &local_); + } + + SensorGateway& gateway() + { + return gateway_; + } + +private: + robot_costmap_2d::LayeredCostmap global_; + robot_costmap_2d::LayeredCostmap local_; + SensorGateway gateway_; +}; + +robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F) +{ + robot_sensor_msgs::LaserScan scan; + scan.header.frame_id = "laser"; + scan.angle_min = -1.5F; // [rad] + scan.angle_max = 1.5F; // [rad] + scan.angle_increment = 3.0F / static_cast(rays); // [rad] + scan.range_min = 0.05F; // [m] + scan.range_max = 10.0F; // [m] + scan.ranges.assign(rays, range); + return scan; +} + +robot_sensor_msgs::DepthCameraData::ConstPtr makeDepth() +{ + robot_sensor_msgs::DepthCameraData::Ptr data = + boost::make_shared(); + data->header.frame_id = "camera_optical"; + return data; +} + +} // namespace + +// ================================================================================================ +// Contract ẩn #1 — kiểu phải tới nơi ĐÚNG NHƯ khi gửi đi +// ================================================================================================ + +TEST(SensorGateway, LaserScanArrivesWithLaserScanType) +{ + Bench bench; + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushLaserScan("/b_scan", makeScan()); + + ASSERT_EQ(voxel->count(), 1U); + EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::LaserScan)); +} + +TEST(SensorGateway, DepthCameraDataArrivesAsConstPtrNotAsValue) +{ + // Đây là test đắt nhất của file. `ObstacleLayer::handleImpl` so + // `typeid(DepthCameraData::ConstPtr)`; gửi đi dạng giá trị sẽ khớp `typeid(DepthCameraData)` và + // rơi qua MỌI nhánh if mà không có lỗi biên dịch, không có log — depth camera lặng lẽ ngừng hoạt + // động. Không có cách nào bắt được lỗi đó ngoài việc kiểm đúng type_info tới nơi. + Bench bench; + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushDepthCameraData("/camera/depth/data", makeDepth()); + + ASSERT_EQ(voxel->count(), 1U); + EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr)); + EXPECT_FALSE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData)); +} + +TEST(SensorGateway, StaticMapArrivesWithOccupancyGridType) +{ + Bench bench; + SpyPtr layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map"); + bench.attach(); + + bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + + ASSERT_EQ(layer->count(), 1U); + EXPECT_TRUE(*layer->records()[0].type == typeid(robot_nav_msgs::OccupancyGrid)); +} + +TEST(SensorGateway, PointCloudAndPointCloud2AreDistinctTypes) +{ + Bench bench; + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushPointCloud("/pc", robot_sensor_msgs::PointCloud()); + bench.gateway().pushPointCloud2("/pc2", robot_sensor_msgs::PointCloud2()); + + ASSERT_EQ(voxel->count(), 2U); + EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::PointCloud)); + EXPECT_TRUE(*voxel->records()[1].type == typeid(robot_sensor_msgs::PointCloud2)); +} + +// ================================================================================================ +// Contract ẩn #2 — `name` là khoá topic, phải tới nguyên văn +// ================================================================================================ + +TEST(SensorGateway, TopicNameReachesTheLayerVerbatim) +{ + // Layer so chuỗi này với `map_topic` / `observation_sources[i].topic` trong YAML. Sửa nó dù chỉ + // một ký tự — kể cả thêm/bớt dấu '/' — là mất hẳn một cảm biến, không cảnh báo. + Bench bench; + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushLaserScan("/b_scan", makeScan()); + bench.gateway().pushDepthCameraData("/camera_right/depth/data", makeDepth()); + + ASSERT_EQ(voxel->count(), 2U); + EXPECT_EQ(voxel->records()[0].topic, "/b_scan"); + EXPECT_EQ(voxel->records()[1].topic, "/camera_right/depth/data"); +} + +// ================================================================================================ +// Contract ẩn #3 — bộ lọc chọn layer (C5) +// ================================================================================================ + +TEST(SensorGateway, StaticMapGoesOnlyToStaticLayers) +{ + Bench bench; + SpyPtr static_layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map"); + SpyPtr voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles"); + SpyPtr inflation = bench.addGlobal(LayerType::INFLATION_LAYER, "inflation"); + bench.attach(); + + bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + + EXPECT_EQ(static_layer->count(), 1U); + EXPECT_EQ(voxel->count(), 0U); + EXPECT_EQ(inflation->count(), 0U); +} + +TEST(SensorGateway, ObstacleDataGoesOnlyToVoxelLayers) +{ + Bench bench; + SpyPtr static_layer = bench.addLocal(LayerType::STATIC_LAYER, "navigation_map"); + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + SpyPtr inflation = bench.addLocal(LayerType::INFLATION_LAYER, "inflation"); + bench.attach(); + + bench.gateway().pushLaserScan("/b_scan", makeScan()); + + EXPECT_EQ(voxel->count(), 1U); + EXPECT_EQ(static_layer->count(), 0U); + EXPECT_EQ(inflation->count(), 0U); +} + +TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample) +{ + // Hồi quy cho C5. Bản `move_base` cũ lọc bằng + // getType() == layer_type || getName() == name + // Vế thứ hai là bẫy: đặt tên một layer trùng tên topic thì nó nhận dữ liệu nó không hiểu. Với + // InflationLayer — có handleImpl chỉ biết log error — hậu quả là spam log ở đúng tần số cảm biến. + Bench bench; + SpyPtr trap = bench.addLocal(LayerType::INFLATION_LAYER, "/b_scan"); + SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + 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(voxel->count(), 1U); +} + +TEST(SensorGateway, SampleReachesBothGlobalAndLocalCostmaps) +{ + Bench bench; + SpyPtr global_voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles"); + SpyPtr local_voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushLaserScan("/b_scan", makeScan()); + + EXPECT_EQ(global_voxel->count(), 1U); + EXPECT_EQ(local_voxel->count(), 1U); + EXPECT_EQ(bench.gateway().stats().delivered, 2U); +} + +// ================================================================================================ +// Layer tắt (C8) và các nhánh bỏ mẫu — phải ĐẾM được, không im lặng +// ================================================================================================ + +TEST(SensorGateway, DisabledLayerIsSkippedAndCounted) +{ + Bench bench; + SpyPtr disabled = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles", /*enabled=*/false); + bench.attach(); + + bench.gateway().pushLaserScan("/b_scan", makeScan()); + + EXPECT_EQ(disabled->count(), 0U); + EXPECT_EQ(bench.gateway().stats().skipped_disabled, 1U); + EXPECT_EQ(bench.gateway().stats().delivered, 0U); +} + +TEST(SensorGateway, SamplesBeforeAnyCostmapIsAttachedAreCounted) +{ + // Bản cũ mở đầu bằng `if (!costmap) return;` không log. Mọi mẫu tới trước khi costmap tồn tại + // biến mất không dấu vết, và đó là trạng thái BÌNH THƯỜNG lúc khởi động. + SensorGateway gateway; + std::string error; + ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error; + ASSERT_FALSE(gateway.attached()); + + gateway.pushStaticMap("/map", robot_nav_msgs::OccupancyGrid()); + gateway.pushLaserScan("/b_scan", makeScan()); + gateway.pushDepthCameraData("/camera/depth/data", makeDepth()); + + EXPECT_EQ(gateway.stats().dropped_no_costmap, 3U); + EXPECT_EQ(gateway.stats().delivered, 0U); +} + +TEST(SensorGateway, NullDepthPointerIsIgnoredWithoutCountingAsADrop) +{ + Bench bench; + bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + bench.gateway().pushDepthCameraData("/camera/depth/data", + robot_sensor_msgs::DepthCameraData::ConstPtr()); + + EXPECT_EQ(bench.gateway().stats().delivered, 0U); + EXPECT_EQ(bench.gateway().stats().dropped_no_costmap, 0U); +} + +TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes) +{ + // Bản cũ bọc try/catch quanh CẢ vòng lặp rồi `return`, nên một layer ném exception làm mọi layer + // đứng sau nó mất luôn mẫu đó — và với một layer hỏng cố định thì mất vĩnh viễn. + Bench bench; + SpyPtr exploding = bench.addLocal(LayerType::VOXEL_LAYER, "broken", true, /*explode=*/true); + SpyPtr healthy = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles"); + bench.attach(); + + 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(bench.gateway().stats().layer_exceptions, 1U); + EXPECT_EQ(bench.gateway().stats().delivered, 1U); +} + +// ================================================================================================ +// Lọc laser (C7) +// ================================================================================================ + +TEST(SensorGatewayConfigTest, LaserFilterIsOffByDefaultAndLeavesTheScanUntouched) +{ + SensorGateway gateway; + std::string error; + ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error; + + const robot_sensor_msgs::LaserScan scan = makeScan(); + const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan); + + EXPECT_EQ(prepared.ranges, scan.ranges); + EXPECT_EQ(prepared.angle_increment, scan.angle_increment); +} + +TEST(SensorGatewayConfigTest, EnabledLaserFilterKeepsScanStructure) +{ + // Bộ lọc giữ nguyên cấu trúc scan (outlier thành NaN) — chỉ số tia phải khớp một-một với góc, nếu + // không thì mọi phép chiếu tia sang điểm sau đó đều lệch. + SensorGatewayConfig config; + config.laser_sor_enabled = true; + config.laser_sor_mean_k = 5; + config.laser_sor_stddev_mul = 1.0; + + SensorGateway gateway; + std::string error; + ASSERT_TRUE(gateway.configure(config, error)) << error; + + const robot_sensor_msgs::LaserScan scan = makeScan(); + const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan); + + EXPECT_EQ(prepared.ranges.size(), scan.ranges.size()); + EXPECT_EQ(prepared.angle_increment, scan.angle_increment); + EXPECT_EQ(prepared.header.frame_id, scan.header.frame_id); +} + +TEST(SensorGatewayConfigTest, RejectsOutOfRangeFilterParametersOnlyWhenFilterIsOn) +{ + std::string error; + + SensorGatewayConfig off; + off.laser_sor_enabled = false; + off.laser_sor_mean_k = 0; // vô nghĩa, nhưng tính năng đang tắt + off.laser_sor_stddev_mul = -1.0; // vô nghĩa, nhưng tính năng đang tắt + EXPECT_TRUE(off.validate(error)) << error; + + SensorGatewayConfig bad_k; + bad_k.laser_sor_enabled = true; + bad_k.laser_sor_mean_k = 1; + EXPECT_FALSE(bad_k.validate(error)); + + SensorGatewayConfig bad_mul; + bad_mul.laser_sor_enabled = true; + bad_mul.laser_sor_stddev_mul = 0.0; + EXPECT_FALSE(bad_mul.validate(error)); +} + +TEST(SensorGatewayConfigTest, ConfigureFailsAndReportsWhyOnInvalidParameters) +{ + SensorGatewayConfig config; + config.laser_sor_enabled = true; + config.laser_sor_mean_k = 0; + + SensorGateway gateway; + std::string error; + EXPECT_FALSE(gateway.configure(config, error)); + EXPECT_FALSE(error.empty()); +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/spy_layer.h b/test/spy_layer.h new file mode 100644 index 0000000..87c3a65 --- /dev/null +++ b/test/spy_layer.h @@ -0,0 +1,116 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — layer gián điệp dùng chung cho các test đường vào cảm biến. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_TEST_SPY_LAYER_H_ +#define MOVE_BASE2_TEST_SPY_LAYER_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace move_base2 +{ +namespace testing +{ + +/** + * @class SpyLayer + * @brief Layer chỉ ghi lại những gì nó nhận được. + * + * Dùng với `LayeredCostmap` **thật**: ba contract ẩn cần khoá lại (type erasure qua `void*` + + * `type_info`, `name` là khoá topic, bộ lọc chọn layer) đều nằm trong lớp thật, nên một costmap giả + * chỉ mô phỏng lại chúng theo cách người viết test *nghĩ* chúng hoạt động. + * + * @note `Layer::Layer()` đặt `enabled_ = false`; layer thật bật cờ này khi đọc config. Ở đây phải tự + * bật — và chính chỗ đó cho phép test nhánh "bỏ qua layer đang tắt". + */ +class SpyLayer : public robot_costmap_2d::Layer +{ +public: + struct Record + { + const std::type_info* type = nullptr; ///< type_info có storage tĩnh nên giữ con trỏ là an toàn. + std::string topic; + }; + + /// @brief Hook để test tự sao chép phần dữ liệu nó quan tâm — con trỏ `data` treo sau khi trả về. + using Observer = std::function; + + explicit SpyLayer(robot_costmap_2d::LayerType type, bool enabled = true, bool explode = false) + : type_(type), explode_(explode) + { + enabled_ = enabled; + } + + robot_costmap_2d::LayerType getType() const override + { + return type_; + } + + void setObserver(Observer observer) + { + observer_ = std::move(observer); + } + + const std::vector& records() const + { + return records_; + } + + std::size_t count() const + { + return records_.size(); + } + +protected: + void handleImpl(const void* data, const std::type_info& type, const std::string& topic) override + { + if (explode_) + { + throw std::runtime_error("SpyLayer được yêu cầu ném exception"); + } + records_.push_back(Record{ &type, topic }); + if (observer_) + { + observer_(data, type, topic); + } + } + +private: + robot_costmap_2d::LayerType type_; + bool explode_; + Observer observer_; + std::vector records_; +}; + +using SpyPtr = boost::shared_ptr; + +/// @brief Cắm một layer gián điệp vào @p costmap và trả con trỏ để kiểm tra sau. +inline SpyPtr attachSpy(robot_costmap_2d::LayeredCostmap& costmap, + robot_costmap_2d::LayerType type, const std::string& name, + bool enabled = true, bool explode = false) +{ + SpyPtr spy = boost::make_shared(type, enabled, explode); + spy->initialize(&costmap, name, nullptr); + costmap.addPlugin(spy); + return spy; +} + +} // namespace testing +} // namespace move_base2 + +#endif // MOVE_BASE2_TEST_SPY_LAYER_H_ diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp new file mode 100644 index 0000000..3f3c2f7 --- /dev/null +++ b/test/state_machine_test.cpp @@ -0,0 +1,1257 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test bảng chuyển state. + * + * Mỗi transition trong docs/STATE_MACHINE.md phải có ít nhất một test ở đây. Bảng đó là nguồn + * chuẩn; test này là thứ chứng minh code khớp với bảng. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include + +using move_base2::ActionFeedback; +using move_base2::ControllerFeedback; +using move_base2::NavigationOutcome; +using move_base2::NavigationState; +using move_base2::PlannerFeedback; +using move_base2::RecoveryFeedback; +using move_base2::RecoveryOutputKind; +using move_base2::RecoveryTrigger; +using move_base2::StateMachine; +using move_base2::StateMachineConfig; +using move_base2::StateMachineInput; +using move_base2::StateMachineOutput; +using move_base2::VelocitySource; + +namespace +{ + +/// @brief Cấu hình cơ sở cho test: ngưỡng ngắn để không phải tua thời gian dài. +StateMachineConfig baseConfig() +{ + StateMachineConfig config; + config.planner_patience = 1.0; // [s] + config.controller_patience = 2.0; // [s] + config.oscillation_timeout = 0.0; // tắt trừ khi test bật riêng + config.oscillation_distance = 0.5; // [m] + config.max_planning_retries = -1; + config.recovery_behavior_count = 2; + config.recovery_enabled = true; + return config; +} + +/** + * @class Driver + * @brief Vỏ mỏng quanh StateMachine để test đọc như kịch bản chứ không như lời gọi API. + */ +class Driver +{ +public: + explicit Driver(const StateMachineConfig& config = baseConfig()) + { + std::string error; + configured_ = machine_.configure(config, error); + EXPECT_TRUE(configured_) << error; + now_ = 1000.0; + } + + /// @param seconds [s] Thời gian trôi trước cycle này. + Driver& advance(double seconds) + { + now_ += seconds; + return *this; + } + + StateMachineOutput tick(StateMachineInput input) + { + input.now = robot::Time(now_); + last_ = machine_.update(input); + return last_; + } + + /// @brief Cycle không có sự kiện gì. + StateMachineOutput idleTick() + { + return tick(StateMachineInput()); + } + + /// @brief Đưa máy từ kIdle tới kControlling: nhận yêu cầu rồi cấp một plan hợp lệ. + void driveToControlling() + { + StateMachineInput request; + request.has_pending_request = true; + ASSERT_EQ(tick(request).state, NavigationState::kPlanning); + + StateMachineInput plan_ready; + plan_ready.planner = PlannerFeedback::kPlanReady; + ASSERT_EQ(tick(plan_ready).state, NavigationState::kControlling); + } + + StateMachine& machine() + { + return machine_; + } + + const StateMachineOutput& last() const + { + return last_; + } + + double now() const + { + return now_; + } + +private: + StateMachine machine_; + StateMachineOutput last_; + double now_ = 1000.0; + bool configured_ = false; +}; + +StateMachineInput planReady() +{ + StateMachineInput input; + input.planner = PlannerFeedback::kPlanReady; + return input; +} + +StateMachineInput plannerFailed() +{ + StateMachineInput input; + input.planner = PlannerFeedback::kFailed; + return input; +} + +StateMachineInput controller(ControllerFeedback feedback) +{ + StateMachineInput input; + input.controller = feedback; + return input; +} + +StateMachineInput recovery(RecoveryFeedback feedback) +{ + StateMachineInput input; + input.recovery = feedback; + return input; +} + +StateMachineInput actionFb(ActionFeedback feedback) +{ + StateMachineInput input; + input.action = feedback; + return input; +} + +/// @brief Yêu cầu mới với hình dạng D8: có goal hay không, và bao nhiêu action. +StateMachineInput requestWithActions(std::size_t action_count, bool has_goal = true) +{ + StateMachineInput input; + input.has_pending_request = true; + input.pending_request_has_goal = has_goal; + input.pending_request_action_count = action_count; + return input; +} + +} // namespace + +// ================================================================================================ +// Cấu hình +// ================================================================================================ + +TEST(StateMachineConfig, RejectsOscillationTimeoutWithoutDistance) +{ + StateMachineConfig config = baseConfig(); + config.oscillation_timeout = 5.0; + config.oscillation_distance = 0.0; + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("oscillation_distance"), std::string::npos); +} + +TEST(StateMachineConfig, RejectsNegativeOscillationDistance) +{ + StateMachineConfig config = baseConfig(); + config.oscillation_distance = -1.0; + + std::string error; + EXPECT_FALSE(config.validate(error)); +} + +TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors) +{ + StateMachineConfig config = baseConfig(); + config.recovery_behavior_count = 0; + 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"; +} + +TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors) +{ + StateMachineConfig config = baseConfig(); + config.recovery_behavior_count = 0; + config.recovery_enabled = false; + + std::string error; + EXPECT_TRUE(config.validate(error)) << error; +} + +TEST(StateMachineConfig, DescribeMentionsEveryParameter) +{ + const std::string text = baseConfig().describe(); + EXPECT_NE(text.find("planner_patience"), std::string::npos); + EXPECT_NE(text.find("controller_patience"), std::string::npos); + EXPECT_NE(text.find("oscillation_timeout"), std::string::npos); + EXPECT_NE(text.find("action_patience"), std::string::npos); + EXPECT_NE(text.find("max_planning_retries"), std::string::npos); +} + +TEST(StateMachine, RefusesToRunBeforeConfigure) +{ + StateMachine machine; + EXPECT_FALSE(machine.initialized()); + + StateMachineInput input; + input.has_pending_request = true; + const StateMachineOutput out = machine.update(input); + + EXPECT_EQ(out.state, NavigationState::kIdle); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); + EXPECT_FALSE(out.accept_request); +} + +TEST(StateMachine, ConfigureFailsLoudlyOnBadConfig) +{ + StateMachineConfig config = baseConfig(); + config.oscillation_timeout = 5.0; + config.oscillation_distance = 0.0; + + StateMachine machine; + std::string error; + EXPECT_FALSE(machine.configure(config, error)); + EXPECT_FALSE(machine.initialized()); + EXPECT_FALSE(error.empty()); +} + +// ================================================================================================ +// IDLE +// ================================================================================================ + +TEST(StateMachineIdle, StaysIdleWithoutRequest) +{ + Driver driver; + const StateMachineOutput out = driver.idleTick(); + + EXPECT_EQ(out.state, NavigationState::kIdle); + EXPECT_FALSE(out.state_changed); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineIdle, RequestMovesToPlanningAndStartsPlanner) +{ + Driver driver; + StateMachineInput input; + input.has_pending_request = true; + + const StateMachineOutput out = driver.tick(input); + + EXPECT_EQ(out.state, NavigationState::kPlanning); + EXPECT_TRUE(out.state_changed); + EXPECT_TRUE(out.accept_request); + EXPECT_TRUE(out.start_planner); + EXPECT_TRUE(out.reset_oscillation_origin); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineIdle, CancelWithoutRequestIsIgnored) +{ + Driver driver; + StateMachineInput input; + input.cancel_requested = true; + + const StateMachineOutput out = driver.tick(input); + EXPECT_EQ(out.state, NavigationState::kIdle); + EXPECT_FALSE(out.report_outcome); +} + +// ================================================================================================ +// PLANNING +// ================================================================================================ + +TEST(StateMachinePlanning, PlanReadyMovesToControllingAndAppliesPlan) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + const StateMachineOutput out = driver.tick(planReady()); + + EXPECT_EQ(out.state, NavigationState::kControlling); + EXPECT_TRUE(out.apply_plan); + EXPECT_TRUE(out.run_controller); + EXPECT_EQ(out.velocity_source, VelocitySource::kController); +} + +TEST(StateMachinePlanning, PlannerPatienceEscalatesToRecovery) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + driver.advance(0.5); + EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning); + + driver.advance(0.6); // tổng 1.1 s > planner_patience = 1.0 s + const StateMachineOutput out = driver.tick(plannerFailed()); + + EXPECT_EQ(out.state, NavigationState::kRecovering); + EXPECT_TRUE(out.start_recovery); + EXPECT_EQ(out.recovery_index, 0u); + EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed); + EXPECT_TRUE(out.stop_planner); +} + +TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires) +{ + StateMachineConfig config = baseConfig(); + config.max_planning_retries = 1; + config.planner_patience = 100.0; // dài, để chắc chắn cái chặn là số lượt chứ không phải thời gian + Driver driver(config); + + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning); // retries = 1 + const StateMachineOutput out = driver.tick(plannerFailed()); // retries = 2 > 1 + + EXPECT_EQ(out.state, NavigationState::kRecovering); + 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 + // thread riêng, tắt CẢ HAI cận (kiên nhẫn và số lượt) bị `validate()` từ chối — không còn gì phát + // hiện được planner treo. Test này kiểm đúng thứ nó vẫn luôn kiểm: đồng hồ kiên nhẫn không kêu. + // Cận số lượt đặt cao hơn hẳn số vòng lặp để nó không phải là thứ kết thúc test. + StateMachineConfig config = baseConfig(); + config.planner_patience = 0.0; + config.max_planning_retries = 1000; + Driver driver(config); + + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + for (int i = 0; i < 100; ++i) + { + driver.advance(1.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "vòng " << i; + } +} + +TEST(StateMachinePlanning, CancelMovesToCancelling) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + StateMachineInput cancel; + cancel.cancel_requested = true; + const StateMachineOutput out = driver.tick(cancel); + + EXPECT_EQ(out.state, NavigationState::kCancelling); + EXPECT_TRUE(out.stop_planner); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachinePlanning, PauseMovesToPausedAndResumeComesBack) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + StateMachineInput pause; + pause.pause_requested = true; + EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + StateMachineInput resume; + resume.resume_requested = true; + const StateMachineOutput out = driver.tick(resume); + + EXPECT_EQ(out.state, NavigationState::kPlanning); + EXPECT_TRUE(out.start_planner); +} + +TEST(StateMachinePlanning, NoRecoveryAvailableAbortsInsteadOfHanging) +{ + StateMachineConfig config = baseConfig(); + config.recovery_enabled = false; + config.recovery_behavior_count = 0; + Driver driver(config); + + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + driver.advance(2.0); + const StateMachineOutput out = driver.tick(plannerFailed()); + + EXPECT_EQ(out.state, NavigationState::kAborted); + EXPECT_TRUE(out.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kFailed); +} + +// ================================================================================================ +// CONTROLLING +// ================================================================================================ + +TEST(StateMachineControlling, GoalReachedFinishesSucceeded) +{ + Driver driver; + driver.driveToControlling(); + + const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kGoalReached)); + + EXPECT_EQ(out.state, NavigationState::kSucceeded); + EXPECT_TRUE(out.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kSucceeded); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineControlling, ValidCommandKeepsControlling) +{ + Driver driver; + driver.driveToControlling(); + + const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid)); + + EXPECT_EQ(out.state, NavigationState::kControlling); + EXPECT_TRUE(out.run_controller); + EXPECT_EQ(out.velocity_source, VelocitySource::kController); +} + +TEST(StateMachineControlling, NoValidCommandGoesBackToPlanning) +{ + Driver driver; + driver.driveToControlling(); + + const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kNoValidCommand)); + + EXPECT_EQ(out.state, NavigationState::kPlanning); + EXPECT_TRUE(out.start_planner); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop) +{ + // Vòng lặp CONTROLLING -> PLANNING -> CONTROLLING không được làm mới đồng hồ controller_patience, + // nếu không thì controller hỏng vĩnh viễn sẽ không bao giờ dẫn tới recovery. + Driver driver; + driver.driveToControlling(); + + for (int i = 0; i < 20; ++i) + { + driver.advance(0.15); + const NavigationState state = driver.tick(controller(ControllerFeedback::kNoValidCommand)).state; + if (state == NavigationState::kRecovering) + { + EXPECT_EQ(driver.last().recovery_trigger, RecoveryTrigger::kControllingFailed); + SUCCEED(); + return; + } + ASSERT_EQ(state, NavigationState::kPlanning) << "vòng " << i; + + driver.advance(0.05); + ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "vòng " << 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ồ"; +} + +TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery) +{ + StateMachineConfig config = baseConfig(); + config.oscillation_timeout = 1.0; // [s] + config.oscillation_distance = 0.5; // [m] + Driver driver(config); + driver.driveToControlling(); + + // Robot vẫn sinh được lệnh nhưng không đi đâu cả. + driver.advance(0.5); + ASSERT_EQ(driver.tick(controller(ControllerFeedback::kCommandValid)).state, + NavigationState::kControlling); + + driver.advance(0.6); // tổng 1.1 s > oscillation_timeout + const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid)); + + EXPECT_EQ(out.state, NavigationState::kRecovering); + EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation); +} + +TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock) +{ + StateMachineConfig config = baseConfig(); + config.oscillation_timeout = 1.0; + config.oscillation_distance = 0.5; + Driver driver(config); + driver.driveToControlling(); + + for (int i = 0; i < 20; ++i) + { + driver.advance(0.5); + StateMachineInput input = controller(ControllerFeedback::kCommandValid); + 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; + } +} + +TEST(StateMachineControlling, NewPlanIsAppliedWithoutLeavingControlling) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput input = planReady(); + input.controller = ControllerFeedback::kCommandValid; + const StateMachineOutput out = driver.tick(input); + + EXPECT_EQ(out.state, NavigationState::kControlling); + EXPECT_TRUE(out.apply_plan); +} + +TEST(StateMachineControlling, CancelMovesToCancelling) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput cancel = controller(ControllerFeedback::kCommandValid); + cancel.cancel_requested = true; + + const StateMachineOutput out = driver.tick(cancel); + EXPECT_EQ(out.state, NavigationState::kCancelling); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineControlling, PauseThenResumeReturnsToControlling) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput pause; + pause.pause_requested = true; + EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + StateMachineInput resume; + resume.resume_requested = true; + const StateMachineOutput out = driver.tick(resume); + + EXPECT_EQ(out.state, NavigationState::kControlling); + EXPECT_TRUE(out.run_controller); +} + +TEST(StateMachineControlling, LongPauseDoesNotBlowControllerPatience) +{ + // Tạm dừng 60 giây rồi tiếp tục không được bị tính là "controller hỏng 60 giây". + Driver driver; + driver.driveToControlling(); + + StateMachineInput pause; + pause.pause_requested = true; + ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + driver.advance(60.0); + + StateMachineInput resume; + resume.resume_requested = true; + ASSERT_EQ(driver.tick(resume).state, NavigationState::kControlling); + + const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid)); + EXPECT_EQ(out.state, NavigationState::kControlling); +} + +TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput blind = controller(ControllerFeedback::kCommandValid); + blind.pose_available = false; + + 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"; + 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. + driver.advance(3.0); + const StateMachineOutput out = driver.tick(blind); + EXPECT_EQ(out.state, NavigationState::kRecovering); + EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kControllingFailed); +} + +// ================================================================================================ +// RECOVERING +// ================================================================================================ + +TEST(StateMachineRecovering, TicksWhileRunningAndOwnsVelocity) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + + const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kRunning)); + + EXPECT_EQ(out.state, NavigationState::kRecovering); + EXPECT_TRUE(out.tick_recovery); + EXPECT_FALSE(out.run_controller); + EXPECT_EQ(out.velocity_source, VelocitySource::kRecovery); +} + +TEST(StateMachineRecovering, OneShotBehaviorDoesNotOwnVelocity) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + + // Behavior đang chạy là loại không lái robot (đợi, xoá costmap). + StateMachineInput failed = plannerFailed(); + failed.active_recovery_output = RecoveryOutputKind::kNone; + ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering); + + // Ngay tại cycle KHỞI ĐỘNG recovery đã phải là kNone, không chờ tới tick sau: đổi nguồn vận tốc + // qua lại tốn một cycle zero mỗi lần, và ở đây không có vận tốc nào để trao quyền. + EXPECT_TRUE(driver.last().start_recovery); + EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone); + + StateMachineInput running = recovery(RecoveryFeedback::kRunning); + running.active_recovery_output = RecoveryOutputKind::kNone; + const StateMachineOutput out = driver.tick(running); + + EXPECT_EQ(out.state, NavigationState::kRecovering); + EXPECT_TRUE(out.tick_recovery); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineRecovering, PathBehaviorDoesNotOwnVelocityEither) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + + StateMachineInput failed = plannerFailed(); + failed.active_recovery_output = RecoveryOutputKind::kPath; + ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering); + + EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineRecovering, SucceededGoesBackToPlanningWithNextIndex) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + ASSERT_EQ(driver.last().recovery_index, 0u); + + const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kSucceeded)); + + EXPECT_EQ(out.state, NavigationState::kPlanning); + EXPECT_TRUE(out.start_planner); + EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u); +} + +TEST(StateMachineRecovering, FailedAlsoAdvancesToNextBehavior) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + + EXPECT_EQ(driver.tick(recovery(RecoveryFeedback::kFailed)).state, NavigationState::kPlanning); + EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u); +} + +TEST(StateMachineRecovering, ExhaustingAllBehaviorsAborts) +{ + StateMachineConfig config = baseConfig(); + config.recovery_behavior_count = 2; + Driver driver(config); + + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + // Behavior 0 + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + ASSERT_EQ(driver.last().recovery_index, 0u); + ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning); + + // Behavior 1 + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + ASSERT_EQ(driver.last().recovery_index, 1u); + ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning); + + // Hết behavior + driver.advance(2.0); + const StateMachineOutput out = driver.tick(plannerFailed()); + + EXPECT_EQ(out.state, NavigationState::kAborted); + EXPECT_TRUE(out.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kFailed); +} + +TEST(StateMachineRecovering, CancelMidRecoveryStopsBehaviorAndCancels) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + + StateMachineInput cancel = recovery(RecoveryFeedback::kRunning); + cancel.cancel_requested = true; + const StateMachineOutput out = driver.tick(cancel); + + EXPECT_EQ(out.state, NavigationState::kCancelling); + EXPECT_TRUE(out.cancel_recovery); + EXPECT_EQ(out.velocity_source, VelocitySource::kNone); +} + +TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + + StateMachineInput pause = recovery(RecoveryFeedback::kRunning); + pause.pause_requested = true; + const StateMachineOutput paused = driver.tick(pause); + + 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"; + + StateMachineInput resume; + resume.resume_requested = true; + EXPECT_EQ(driver.tick(resume).state, NavigationState::kPlanning); +} + +TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + + StateMachineInput blind = recovery(RecoveryFeedback::kRunning); + blind.pose_available = false; + 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_EQ(out.velocity_source, VelocitySource::kNone); +} + +// ================================================================================================ +// PAUSED / CANCELLING / terminal +// ================================================================================================ + +TEST(StateMachinePaused, CancelFromPausedGoesToCancelling) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput pause; + pause.pause_requested = true; + ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + StateMachineInput cancel; + cancel.cancel_requested = true; + EXPECT_EQ(driver.tick(cancel).state, NavigationState::kCancelling); +} + +TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput pause; + pause.pause_requested = true; + ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + for (int i = 0; i < 50; ++i) + { + driver.advance(1.0); + ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "vòng " << i; + ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone); + } +} + +TEST(StateMachineCancelling, WaitsForRobotToStopBeforeCancelled) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput cancel; + cancel.cancel_requested = true; + ASSERT_EQ(driver.tick(cancel).state, NavigationState::kCancelling); + + StateMachineInput still_moving; + still_moving.robot_stopped = false; + EXPECT_EQ(driver.tick(still_moving).state, NavigationState::kCancelling); + EXPECT_FALSE(driver.last().report_outcome); + + StateMachineInput stopped; + stopped.robot_stopped = true; + const StateMachineOutput out = driver.tick(stopped); + + EXPECT_EQ(out.state, NavigationState::kCancelled); + EXPECT_TRUE(out.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled); +} + +TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce) +{ + Driver driver; + driver.driveToControlling(); + + const StateMachineOutput finish = driver.tick(controller(ControllerFeedback::kGoalReached)); + ASSERT_TRUE(finish.report_outcome); + + // Mọi cycle sau đó không được báo lại lần nữa. + 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_EQ(out.state, NavigationState::kIdle); + } +} + +TEST(StateMachineTerminal, NewRequestAfterTerminalStartsFreshWithRecoveryIndexZero) +{ + Driver driver; + StateMachineInput request; + request.has_pending_request = true; + driver.tick(request); + + // Đốt hết behavior để ABORTED. + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering); + ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning); + driver.advance(2.0); + ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kAborted); + + // Yêu cầu mới phải bắt đầu lại từ behavior 0. + const StateMachineOutput out = driver.tick(request); + EXPECT_EQ(out.state, NavigationState::kPlanning); + EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u); +} + +TEST(StateMachineTerminal, TerminalStateLastsExactlyOneCycle) +{ + Driver driver; + driver.driveToControlling(); + + ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state, + NavigationState::kSucceeded); + EXPECT_EQ(driver.idleTick().state, NavigationState::kIdle); +} + +// ================================================================================================ +// EXECUTING_ACTIONS (D8) — action chạy sau goal, hoặc thay goal khi yêu cầu chỉ-có-action +// ================================================================================================ + +TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely) +{ + Driver driver; + + const StateMachineOutput accepted = driver.tick(requestWithActions(1, /*has_goal=*/false)); + EXPECT_EQ(accepted.state, NavigationState::kExecutingActions); + 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_EQ(accepted.velocity_source, VelocitySource::kNone); + + ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state, + NavigationState::kExecutingActions); + EXPECT_TRUE(driver.last().tick_action); + + const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded)); + EXPECT_EQ(done.state, NavigationState::kSucceeded); + EXPECT_TRUE(done.report_outcome); + EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded); +} + +TEST(StateMachineActions, RequestWithoutGoalOrActionsAbortsDefensively) +{ + Driver driver; + + const StateMachineOutput out = driver.tick(requestWithActions(0, /*has_goal=*/false)); + EXPECT_EQ(out.state, NavigationState::kAborted); + EXPECT_TRUE(out.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kFailed); + EXPECT_FALSE(out.start_action); +} + +TEST(StateMachineActions, ActionsRunInOrderAfterGoalReached) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(2)).state, NavigationState::kPlanning); + ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling); + + // Tới goal: chưa được SUCCEEDED — còn 2 action phải chạy, và chưa được báo kết quả. + const StateMachineOutput at_goal = driver.tick(controller(ControllerFeedback::kGoalReached)); + ASSERT_EQ(at_goal.state, NavigationState::kExecutingActions); + EXPECT_FALSE(at_goal.report_outcome); + EXPECT_TRUE(at_goal.start_action); + EXPECT_EQ(at_goal.action_index, 0u); + + // Action 0 xong -> khởi động action 1, vẫn ở kExecutingActions. + const StateMachineOutput next = driver.tick(actionFb(ActionFeedback::kSucceeded)); + ASSERT_EQ(next.state, NavigationState::kExecutingActions); + EXPECT_TRUE(next.start_action); + EXPECT_EQ(next.action_index, 1u); + EXPECT_FALSE(next.report_outcome); + + // Action cuối xong -> SUCCEEDED, báo kết quả đúng một lần. + const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded)); + EXPECT_EQ(done.state, NavigationState::kSucceeded); + EXPECT_TRUE(done.report_outcome); + EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded); +} + +TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(1)).state, NavigationState::kPlanning); + ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling); + ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state, + NavigationState::kExecutingActions); + + const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kFailed)); + 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"; +} + +TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + StateMachineInput cancel; + cancel.cancel_requested = true; + const StateMachineOutput cancelling = driver.tick(cancel); + EXPECT_EQ(cancelling.state, NavigationState::kCancelling); + EXPECT_TRUE(cancelling.cancel_action); + + StateMachineInput stopped; + stopped.robot_stopped = true; + const StateMachineOutput out = driver.tick(stopped); + EXPECT_EQ(out.state, NavigationState::kCancelled); + EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled); +} + +TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + StateMachineInput pause; + 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.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ở"; +} + +TEST(StateMachineActions, ActionPatienceIsDisabledByDefault) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + // Action dài hợp lệ (ví dụ sạc pin): mặc định không có trần nào ở tầng navigation. + driver.advance(3600.0); + const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning)); + EXPECT_EQ(out.state, NavigationState::kExecutingActions); + EXPECT_TRUE(out.tick_action); +} + +TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort) +{ + StateMachineConfig config = baseConfig(); + config.action_patience = 1.0; // [s] + + Driver driver(config); + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + driver.advance(0.5); + ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state, + NavigationState::kExecutingActions); + + 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.report_outcome); + EXPECT_EQ(out.outcome, NavigationOutcome::kFailed); +} + +TEST(StateMachineActions, PauseRearmsActionPatienceClock) +{ + StateMachineConfig config = baseConfig(); + config.action_patience = 1.0; // [s] + + Driver driver(config); + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + StateMachineInput pause; + pause.pause_requested = true; + ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused); + + // Dừng lâu hơn hẳn action_patience rồi mới resume — quãng dừng không được tính vào trần. + driver.advance(30.0); + StateMachineInput resume; + resume.resume_requested = true; + ASSERT_EQ(driver.tick(resume).state, NavigationState::kExecutingActions); + + 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_TRUE(out.tick_action); +} + +TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero) +{ + Driver driver; + ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state, + NavigationState::kExecutingActions); + + StateMachineInput no_pose; + no_pose.pose_available = false; + 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_EQ(out.velocity_source, VelocitySource::kNone); +} + +// ================================================================================================ +// Bất biến áp cho mọi state +// ================================================================================================ + +TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously) +{ + // Quét một chuỗi sự kiện dài, đủ để đi qua mọi state — kể cả kExecutingActions (D8) — và kiểm + // bất biến ở từng cycle. + Driver driver; + + const StateMachineInput sequence[] = { + requestWithActions(1), + planReady(), + controller(ControllerFeedback::kCommandValid), + controller(ControllerFeedback::kNoValidCommand), + plannerFailed(), + plannerFailed(), + recovery(RecoveryFeedback::kRunning), + recovery(RecoveryFeedback::kSucceeded), + planReady(), + controller(ControllerFeedback::kGoalReached), + actionFb(ActionFeedback::kRunning), + actionFb(ActionFeedback::kSucceeded), + }; + + for (const auto& input : sequence) + { + driver.advance(0.6); + 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"; + ASSERT_FALSE(out.run_controller && out.tick_action) + << "state " << move_base2::toString(out.state) << ": controller chạy cùng action"; + ASSERT_FALSE(out.tick_recovery && out.tick_action) + << "state " << move_base2::toString(out.state) << ": recovery chạy cùng action"; + + if (move_base2::mustBeStopped(out.state)) + { + ASSERT_EQ(out.velocity_source, VelocitySource::kNone) + << "state " << move_base2::toString(out.state) << " phải dừng"; + } + if (out.velocity_source == VelocitySource::kController) + { + ASSERT_EQ(out.state, NavigationState::kControlling); + } + if (out.velocity_source == VelocitySource::kRecovery) + { + ASSERT_EQ(out.state, NavigationState::kRecovering); + } + } +} + +TEST(StateMachineInvariants, ResetReturnsToPristineState) +{ + Driver driver; + driver.driveToControlling(); + driver.tick(controller(ControllerFeedback::kCommandValid)); + + driver.machine().reset(); + + EXPECT_EQ(driver.machine().state(), NavigationState::kIdle); + EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u); + EXPECT_EQ(driver.machine().planningRetries(), 0); +} + +TEST(StateMachineInvariants, EveryStateHasANonEmptyName) +{ + const NavigationState states[] = { + NavigationState::kIdle, NavigationState::kPlanning, NavigationState::kControlling, + NavigationState::kRecovering, NavigationState::kExecutingActions, NavigationState::kPaused, + NavigationState::kCancelling, NavigationState::kSucceeded, NavigationState::kAborted, + NavigationState::kCancelled, + }; + + for (const auto state : states) + { + const std::string name = move_base2::toString(state); + EXPECT_FALSE(name.empty()); + EXPECT_NE(name, "UNKNOWN"); + } +} + +// ================================================================================================ +// kBusy — planner bất đồng bộ +// ================================================================================================ + +TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning) +{ + // kBusy phải hành xử đúng như kIdle ở mọi điểm quyết định: chờ tiếp, KHÔNG tính là lượt hỏng. + Driver driver; + + StateMachineInput request; + request.has_pending_request = true; + ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning); + + StateMachineInput busy; + busy.planner = PlannerFeedback::kBusy; + + for (int i = 1; i <= 5; ++i) + { + driver.advance(0.05); + EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning) << "rời PLANNING ở cycle " << i; + } +} + +TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt) +{ + StateMachineConfig config = baseConfig(); + config.max_planning_retries = 2; + config.planner_patience = 100.0; // [s] loại đường kiên nhẫn để chỉ còn đếm lượt + + Driver driver(config); + + StateMachineInput request; + request.has_pending_request = true; + ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning); + + StateMachineInput busy; + busy.planner = PlannerFeedback::kBusy; + + for (int i = 1; i <= 10; ++i) + { + driver.advance(0.05); + driver.tick(busy); + } + + 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"; +} + +TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan) +{ + Driver driver; + driver.driveToControlling(); + + StateMachineInput busy; + busy.planner = PlannerFeedback::kBusy; + busy.controller = ControllerFeedback::kCommandValid; + + driver.advance(0.05); + 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"; +} + +TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector) +{ + // Từ khi planner chạy trên thread riêng, đây là thứ duy nhất phát hiện được plugin không bao giờ + // trả lời. Ở chế độ đồng bộ trước đây, planner treo làm treo luôn control loop — hỏng thì thấy ngay. + StateMachineConfig config = baseConfig(); + config.planner_patience = 0.0; + config.max_planning_retries = -1; + + std::string error; + EXPECT_FALSE(config.validate(error)); + EXPECT_NE(error.find("planner treo"), std::string::npos) << error; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/velocity_arbiter_test.cpp b/test/velocity_arbiter_test.cpp new file mode 100644 index 0000000..872b9bf --- /dev/null +++ b/test/velocity_arbiter_test.cpp @@ -0,0 +1,398 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test bộ trọng tài vận tốc. + * + * Ba quy tắc phải được chứng minh chứ không chỉ được ghi trong comment: kNone phát 0, mọi lệnh đi + * qua sanitize, và đổi nguồn luôn chèn một cycle 0. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include + +using move_base2::VelocityArbiter; +using move_base2::VelocityLimits; +using move_base2::VelocitySource; + +namespace +{ + +constexpr double kDt = 0.05; ///< [s] chu kỳ dùng trong test + +VelocityLimits baseLimits() +{ + VelocityLimits limits; + limits.max_vel_x = 0.5; // [m/s] + limits.min_vel_x = -0.2; // [m/s] + limits.max_vel_theta = 1.0; // [rad/s] + limits.max_accel_x = 100.0; // [m/s^2] rất lớn: mặc định tắt ảnh hưởng của giới hạn gia tốc + limits.max_accel_theta = 100.0; // [rad/s^2] + limits.zero_velocity_epsilon = 1e-3; + return limits; +} + +robot_geometry_msgs::Twist twist(double linear_x, double angular_z) +{ + robot_geometry_msgs::Twist cmd; + cmd.linear.x = linear_x; + cmd.angular.z = angular_z; + return cmd; +} + +VelocityArbiter makeArbiter(const VelocityLimits& limits = baseLimits()) +{ + VelocityArbiter arbiter; + std::string error; + EXPECT_TRUE(arbiter.configure(limits, error)) << error; + return arbiter; +} + +} // namespace + +// ================================================================================================ +// Cấu hình +// ================================================================================================ + +TEST(VelocityLimits, RejectsNonPositiveMaxVelX) +{ + VelocityLimits limits = baseLimits(); + limits.max_vel_x = 0.0; + + std::string error; + EXPECT_FALSE(limits.validate(error)); + EXPECT_NE(error.find("max_vel_x"), std::string::npos); +} + +TEST(VelocityLimits, RejectsPositiveMinVelXBecauseItIsTheReverseLimit) +{ + VelocityLimits limits = baseLimits(); + limits.min_vel_x = 0.3; + + std::string error; + EXPECT_FALSE(limits.validate(error)); + EXPECT_NE(error.find("min_vel_x"), std::string::npos); +} + +TEST(VelocityLimits, RejectsNonPositiveAccelerations) +{ + std::string error; + + VelocityLimits linear = baseLimits(); + linear.max_accel_x = 0.0; + EXPECT_FALSE(linear.validate(error)); + + VelocityLimits angular = baseLimits(); + angular.max_accel_theta = -1.0; + EXPECT_FALSE(angular.validate(error)); +} + +TEST(VelocityLimits, DescribeMarksReverseAsDisabledWhenZero) +{ + VelocityLimits limits = baseLimits(); + limits.min_vel_x = 0.0; + EXPECT_NE(limits.describe().find("cấm lùi"), std::string::npos); +} + +TEST(VelocityArbiter, RefusesToEmitBeforeConfigure) +{ + VelocityArbiter arbiter; + EXPECT_FALSE(arbiter.initialized()); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt); + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0); +} + +TEST(VelocityArbiter, ConfigureFailsLoudlyOnBadLimits) +{ + VelocityLimits limits = baseLimits(); + limits.max_vel_theta = -1.0; + + VelocityArbiter arbiter; + std::string error; + EXPECT_FALSE(arbiter.configure(limits, error)); + EXPECT_FALSE(arbiter.initialized()); +} + +// ================================================================================================ +// Quy tắc 1 — nguồn kNone phát 0 +// ================================================================================================ + +TEST(VelocityArbiter, NoneSourceEmitsExactZeroImmediately) +{ + VelocityArbiter arbiter = makeArbiter(); + + arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.8), kDt); + ASSERT_GT(arbiter.lastCommand().linear.x, 0.0); + + 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.angular.z, 0.0); + EXPECT_TRUE(arbiter.stopped()); +} + +TEST(VelocityArbiter, NoneSourceIgnoresCandidateEntirely) +{ + VelocityArbiter arbiter = makeArbiter(); + + const auto cmd = arbiter.arbitrate(VelocitySource::kNone, twist(99.0, 99.0), kDt); + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0); + EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone); +} + +// ================================================================================================ +// Quy tắc 2 — sanitize +// ================================================================================================ + +TEST(VelocityArbiter, NaNIsBlockedAndCounted) +{ + VelocityArbiter arbiter = makeArbiter(); + const double nan_value = std::numeric_limits::quiet_NaN(); + + 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_EQ(arbiter.nonFiniteRejections(), 1u); +} + +TEST(VelocityArbiter, InfinityIsBlockedToo) +{ + VelocityArbiter arbiter = makeArbiter(); + const double inf_value = std::numeric_limits::infinity(); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.2, inf_value), kDt); + + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.angular.z, 0.0); + EXPECT_EQ(arbiter.nonFiniteRejections(), 1u); +} + +TEST(VelocityArbiter, ForwardVelocityIsClampedToMax) +{ + VelocityArbiter arbiter = makeArbiter(); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt); + + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.5); + EXPECT_EQ(arbiter.velocityClamps(), 1u); +} + +TEST(VelocityArbiter, ReverseVelocityIsClampedToMinNotToZero) +{ + VelocityArbiter arbiter = makeArbiter(); + + 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"; +} + +TEST(VelocityArbiter, ReverseIsForbiddenWhenMinVelXIsZero) +{ + VelocityLimits limits = baseLimits(); + limits.min_vel_x = 0.0; + VelocityArbiter arbiter = makeArbiter(limits); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(-0.5, 0.0), kDt); + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0); +} + +TEST(VelocityArbiter, YawRateIsClampedBothDirections) +{ + VelocityArbiter arbiter = makeArbiter(); + + EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, 5.0), kDt).angular.z, + 1.0); + EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.0, -5.0), kDt).angular.z, + -1.0); +} + +TEST(VelocityArbiter, LateralAndUnusedAxesAreDropped) +{ + VelocityArbiter arbiter = makeArbiter(); + + robot_geometry_msgs::Twist candidate = twist(0.2, 0.1); + candidate.linear.y = 0.7; + candidate.linear.z = 0.7; + candidate.angular.x = 0.7; + candidate.angular.y = 0.7; + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, candidate, kDt); + + EXPECT_DOUBLE_EQ(cmd.linear.y, 0.0); + EXPECT_DOUBLE_EQ(cmd.linear.z, 0.0); + EXPECT_DOUBLE_EQ(cmd.angular.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.angular.y, 0.0); +} + +TEST(VelocityArbiter, AccelerationIsLimitedByRealDtNotNominalPeriod) +{ + VelocityLimits limits = baseLimits(); + limits.max_accel_x = 1.0; // [m/s^2] + VelocityArbiter arbiter = makeArbiter(limits); + + // dt = 0.05 s -> bước nhảy tối đa 0.05 m/s. + const auto small_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.05); + EXPECT_NEAR(small_step.linear.x, 0.05, 1e-9); + EXPECT_EQ(arbiter.accelerationClamps(), 1u); + + // Cycle chậm gấp 10: dt = 0.5 s -> bước nhảy tối đa 0.5 m/s, nên đạt luôn trần vận tốc. + const auto big_step = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.5); + EXPECT_NEAR(big_step.linear.x, 0.5, 1e-9); +} + +TEST(VelocityArbiter, NonPositiveDtSkipsAccelerationLimitInsteadOfInventingOne) +{ + VelocityLimits limits = baseLimits(); + limits.max_accel_x = 1.0; + VelocityArbiter arbiter = makeArbiter(limits); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), 0.0); + EXPECT_NEAR(cmd.linear.x, 0.5, 1e-9); +} + +TEST(VelocityArbiter, DecelerationIsAlsoLimited) +{ + VelocityLimits limits = baseLimits(); + limits.max_accel_x = 1.0; + VelocityArbiter arbiter = makeArbiter(limits); + + // Tăng dần tới 0.3 m/s. + for (int i = 0; i < 20; ++i) + { + arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), 0.05); + } + ASSERT_NEAR(arbiter.lastCommand().linear.x, 0.3, 1e-6); + + // Yêu cầu về 0 ngay: vẫn cùng nguồn nên bị giới hạn gia tốc chặn lại. + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.0, 0.0), 0.05); + EXPECT_NEAR(cmd.linear.x, 0.25, 1e-6); +} + +// ================================================================================================ +// Quy tắc 3 — đổi nguồn chèn một cycle 0 +// ================================================================================================ + +TEST(VelocityArbiter, SourceHandoverInsertsExactlyOneZeroCycle) +{ + VelocityArbiter arbiter = makeArbiter(); + + const auto controlling = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt); + 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_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"; +} + +TEST(VelocityArbiter, HandoverWorksInBothDirections) +{ + VelocityArbiter arbiter = makeArbiter(); + + arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt); + arbiter.arbitrate(VelocitySource::kRecovery, twist(-0.1, 0.0), kDt); + ASSERT_NEAR(arbiter.lastCommand().linear.x, -0.1, 1e-9); + + EXPECT_DOUBLE_EQ(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x, + 0.0); + EXPECT_NEAR(arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt).linear.x, 0.3, + 1e-9); + EXPECT_EQ(arbiter.handoverCycles(), 1u); +} + +TEST(VelocityArbiter, GoingThroughNoneDoesNotCountAsHandover) +{ + // kController -> kNone -> kController: cycle kNone đã ép về 0 rồi, không cần chèn thêm. + VelocityArbiter arbiter = makeArbiter(); + + arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt); + arbiter.arbitrate(VelocitySource::kNone, twist(0.0, 0.0), kDt); + + const auto resumed = arbiter.arbitrate(VelocitySource::kController, twist(0.4, 0.0), kDt); + EXPECT_NEAR(resumed.linear.x, 0.4, 1e-9); + EXPECT_EQ(arbiter.handoverCycles(), 0u); +} + +TEST(VelocityArbiter, FirstCommandAfterConfigureNeedsNoHandover) +{ + VelocityArbiter arbiter = makeArbiter(); + + const auto cmd = arbiter.arbitrate(VelocitySource::kController, twist(0.3, 0.0), kDt); + EXPECT_NEAR(cmd.linear.x, 0.3, 1e-9); + EXPECT_EQ(arbiter.handoverCycles(), 0u); +} + +// ================================================================================================ +// Dừng khẩn và reset +// ================================================================================================ + +TEST(VelocityArbiter, EmergencyStopIgnoresAccelerationLimit) +{ + VelocityLimits limits = baseLimits(); + limits.max_accel_x = 0.01; // giảm tốc bình thường sẽ mất rất nhiều cycle + VelocityArbiter arbiter = makeArbiter(limits); + + for (int i = 0; i < 100; ++i) + { + arbiter.arbitrate(VelocitySource::kController, twist(0.5, 0.0), kDt); + } + ASSERT_GT(arbiter.lastCommand().linear.x, 0.0); + + const auto cmd = arbiter.emergencyStop(); + EXPECT_DOUBLE_EQ(cmd.linear.x, 0.0); + EXPECT_TRUE(arbiter.stopped()); + EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone); +} + +TEST(VelocityArbiter, ResetClearsCountersAndHistory) +{ + VelocityArbiter arbiter = makeArbiter(); + + arbiter.arbitrate(VelocitySource::kController, + twist(std::numeric_limits::quiet_NaN(), 0.0), kDt); + arbiter.arbitrate(VelocitySource::kController, twist(9.0, 0.0), kDt); + arbiter.arbitrate(VelocitySource::kRecovery, twist(0.1, 0.0), kDt); + ASSERT_GT(arbiter.nonFiniteRejections(), 0u); + ASSERT_GT(arbiter.velocityClamps(), 0u); + ASSERT_GT(arbiter.handoverCycles(), 0u); + + arbiter.reset(); + + EXPECT_EQ(arbiter.nonFiniteRejections(), 0u); + EXPECT_EQ(arbiter.velocityClamps(), 0u); + EXPECT_EQ(arbiter.accelerationClamps(), 0u); + EXPECT_EQ(arbiter.handoverCycles(), 0u); + EXPECT_EQ(arbiter.activeSource(), VelocitySource::kNone); + EXPECT_TRUE(arbiter.stopped()); +} + +TEST(VelocityArbiter, StoppedUsesEpsilonNotExactZero) +{ + VelocityLimits limits = baseLimits(); + limits.zero_velocity_epsilon = 0.01; + VelocityArbiter arbiter = makeArbiter(limits); + + arbiter.arbitrate(VelocitySource::kController, twist(0.005, 0.0), kDt); + EXPECT_TRUE(arbiter.stopped()); + + arbiter.arbitrate(VelocitySource::kController, twist(0.05, 0.0), kDt); + EXPECT_FALSE(arbiter.stopped()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/walking_skeleton_test.cpp b/test/walking_skeleton_test.cpp new file mode 100644 index 0000000..bc8d714 --- /dev/null +++ b/test/walking_skeleton_test.cpp @@ -0,0 +1,1009 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test bộ khung chạy end-to-end với thành phần giả. + * + * Đây là test chứng minh contract giữa các cổng đã khớp: nhận yêu cầu, đi qua đủ state, gọi đúng + * cổng, phát đúng lệnh vận tốc, và báo kết quả đúng một lần. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include + +#include "fake_ports.h" + +using move_base2::ControlLoop; +using move_base2::ControlLoopConfig; +using move_base2::ControlLoopDeps; +using move_base2::MotionProfile; +using move_base2::NavigationRequest; +using move_base2::NavigationState; +using move_base2::RecoveryTrigger; +using move_base2::testing::ActionScript; +using move_base2::testing::ControllerScript; +using move_base2::testing::FakeActionPort; +using move_base2::testing::FakeClockPort; +using move_base2::testing::FakeControllerPort; +using move_base2::testing::FakeMissionPort; +using move_base2::testing::FakePlannerPort; +using move_base2::testing::FakePosePort; +using move_base2::testing::FakeRecoveryPort; +using move_base2::testing::PlannerScript; +using move_base2::testing::RecoveryScript; + +namespace +{ + +constexpr double kControlPeriod = 0.05; ///< [s] + +ControlLoopConfig baseConfig() +{ + ControlLoopConfig config; + + 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 = 2; + config.state_machine.recovery_enabled = true; + + config.velocity.max_vel_x = 0.5; // [m/s] + config.velocity.min_vel_x = -0.2; // [m/s] + config.velocity.max_vel_theta = 1.0; // [rad/s] + config.velocity.max_accel_x = 100.0; // [m/s^2] lớn để test tập trung vào state, không vào ramp + config.velocity.max_accel_theta = 100.0; // [rad/s^2] + + config.nominal_control_period = kControlPeriod; + + 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"; + config.go_straight = config.position; + config.go_straight.local_planner_name = "FakeStraightPlanner"; + config.rotate = config.position; + config.rotate.local_planner_name = "FakeRotatePlanner"; + + return config; +} + +NavigationRequest makeRequest(double goal_x, std::uint64_t sequence_id = 0) +{ + NavigationRequest request; + request.profile = MotionProfile::kPosition; + request.goal.header.frame_id = "map"; + request.goal.pose.position.x = goal_x; + request.goal.pose.orientation.w = 1.0; + request.mission_sequence_id = sequence_id; + return request; +} + +/// @brief Gắn @p count action tên "act_" vào yêu cầu — kiểm được thứ tự tới ActionPort (D8). +NavigationRequest withActions(NavigationRequest request, std::size_t count) +{ + for (std::size_t i = 0; i < count; ++i) + { + robot_protocol_msgs::Action action; + action.actionType = "act_" + std::to_string(i); + request.actions.push_back(action); + } + return request; +} + +/// @brief Yêu cầu chỉ-có-action (D8): has_goal = false, goal để mặc định — không được validate. +NavigationRequest makeActionOnlyRequest(std::size_t count, std::uint64_t sequence_id = 0) +{ + NavigationRequest request; + request.has_goal = false; + request.mission_sequence_id = sequence_id; + return withActions(request, count); +} + +/** + * @class Fixture + * @brief Dựng sẵn control loop nối với toàn bộ cổng giả. + */ +class Fixture +{ +public: + explicit Fixture(const ControlLoopConfig& config = baseConfig()) : recovery_(2) + { + pose_.setPosition(0.0, 0.0); + + deps_.clock = &clock_; + deps_.pose = &pose_; + deps_.planner = &planner_; + deps_.controller = &controller_; + deps_.recovery = &recovery_; + deps_.mission = &mission_; + deps_.action = &action_; + + std::string error; + EXPECT_TRUE(loop_.configure(config, deps_, error)) << error; + } + + /// @brief Chạy tối đa @p max_cycles cycle, ghi lại chuỗi state (đã nén lặp liên tiếp). + void run(std::size_t max_cycles = 200) + { + for (std::size_t i = 0; i < max_cycles; ++i) + { + record(); + if (!loop_.step()) + { + record(); + return; + } + clock_.advance(kControlPeriod); + } + hit_limit_ = true; + } + + /// @brief Chạy đúng một cycle và ghi lại state sau cycle đó. + void stepOnce() + { + loop_.step(); + clock_.advance(kControlPeriod); + record(); + } + + const std::vector& states() const + { + return states_; + } + + bool hitLimit() const + { + return hit_limit_; + } + + ControlLoop loop_; + FakeClockPort clock_; + FakePosePort pose_; + FakePlannerPort planner_; + FakeControllerPort controller_; + FakeRecoveryPort recovery_; + FakeMissionPort mission_; + FakeActionPort action_; + ControlLoopDeps deps_; + +private: + void record() + { + const std::string name = move_base2::toString(loop_.state()); + if (states_.empty() || states_.back() != name) + { + states_.push_back(name); + } + } + + std::vector states_; + bool hit_limit_ = false; +}; + +std::string join(const std::vector& items) +{ + std::string out = "["; + for (std::size_t i = 0; i < items.size(); ++i) + { + out += items[i]; + if (i + 1 < items.size()) + { + out += ", "; + } + } + return out + "]"; +} + +} // namespace + +// ================================================================================================ +// Cấu hình và cửa vào +// ================================================================================================ + +TEST(ControlLoop, RefusesToConfigureWithMissingPorts) +{ + ControlLoop loop; + ControlLoopDeps deps; // toàn null + + std::string error; + EXPECT_FALSE(loop.configure(baseConfig(), deps, error)); + EXPECT_NE(error.find("cổng"), std::string::npos); + EXPECT_FALSE(loop.initialized()); +} + +TEST(ControlLoop, RefusesToConfigureWithInvalidVelocityLimits) +{ + Fixture fixture; + ControlLoopConfig bad = baseConfig(); + bad.velocity.max_vel_x = -1.0; + + ControlLoop loop; + std::string error; + EXPECT_FALSE(loop.configure(bad, fixture.deps_, error)); +} + +TEST(ControlLoop, RefusesToConfigureWithoutPositionPlanner) +{ + Fixture fixture; + ControlLoopConfig bad = baseConfig(); + bad.position.local_planner_name.clear(); + + ControlLoop loop; + std::string error; + EXPECT_FALSE(loop.configure(bad, fixture.deps_, error)); + EXPECT_NE(error.find("position"), std::string::npos); +} + +TEST(ControlLoop, RejectsGoalWithInvalidQuaternion) +{ + Fixture fixture; + NavigationRequest request = makeRequest(3.0); + request.goal.pose.orientation.w = 0.0; // norm = 0 + + std::string reason; + EXPECT_FALSE(fixture.loop_.submit(request, reason)); + EXPECT_NE(reason.find("quaternion"), std::string::npos); +} + +TEST(ControlLoop, RejectsGoalWithNonFiniteCoordinates) +{ + Fixture fixture; + NavigationRequest request = makeRequest(3.0); + request.goal.pose.position.x = std::numeric_limits::quiet_NaN(); + + std::string reason; + EXPECT_FALSE(fixture.loop_.submit(request, reason)); +} + +TEST(ControlLoop, RejectsRequestWhenLocalPlannerCannotBeLoaded) +{ + Fixture fixture; + fixture.controller_.setSwapSucceeds(false); + + std::string reason; + EXPECT_FALSE(fixture.loop_.submit(makeRequest(3.0), reason)); + EXPECT_NE(reason.find("local planner"), std::string::npos); +} + +TEST(ControlLoop, RejectsRequestWhenGlobalPlannerCannotBeLoaded) +{ + Fixture fixture; + fixture.planner_.setSwapSucceeds(false); + + std::string reason; + EXPECT_FALSE(fixture.loop_.submit(makeRequest(3.0), reason)); + EXPECT_NE(reason.find("global planner"), std::string::npos); +} + +TEST(ControlLoop, ProfileSelectsItsOwnLocalPlannerAndTolerances) +{ + Fixture fixture; + + NavigationRequest docking = makeRequest(1.0); + docking.profile = MotionProfile::kDocking; + + 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"; + + 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); +} + +// ================================================================================================ +// Đường đi thuận lợi +// ================================================================================================ + +TEST(ControlLoop, HappyPathReachesSucceededAndReportsOnce) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kOk}); + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk, + ControllerScript::kOk, ControllerScript::kGoalReached}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 42), reason)) << reason; + + fixture.run(); + + EXPECT_FALSE(fixture.hitLimit()); + EXPECT_EQ(fixture.states(), (std::vector{"IDLE", "PLANNING", "CONTROLLING", + "SUCCEEDED"})) + << join(fixture.states()); + 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"; +} + +TEST(ControlLoop, DirectGoalWithoutMissionIdDoesNotTouchMissionLayer) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 0), reason)) << reason; + fixture.run(); + + 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"; +} + +TEST(ControlLoop, ControllerCommandIsPublishedWhileControlling) +{ + Fixture fixture; + fixture.controller_.setNominalSpeed(0.3); + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.loop_.step(); // IDLE -> PLANNING (lập plan trong cycle này) + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); // PLANNING -> CONTROLLING, chạy controller ngay + + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + EXPECT_NEAR(fixture.loop_.lastCommand().linear.x, 0.3, 1e-9); +} + +TEST(ControlLoop, OrderIsForwardedToThePlanner) +{ + Fixture fixture; + NavigationRequest request = makeRequest(3.0); + request.order = std::make_shared(); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(request, reason)) << reason; + + fixture.loop_.step(); + EXPECT_TRUE(fixture.planner_.sawOrder()); +} + +// ================================================================================================ +// Bất biến an toàn +// ================================================================================================ + +TEST(ControlLoop, NoVelocityIsEmittedOutsideControllingAndRecovering) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk, + ControllerScript::kGoalReached}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + for (int i = 0; i < 50; ++i) + { + const bool running = fixture.loop_.step(); + const NavigationState state = fixture.loop_.state(); + + if (move_base2::mustBeStopped(state)) + { + ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0) + << "state " << move_base2::toString(state) << " ở cycle " << i; + ASSERT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 0.0) + << "state " << move_base2::toString(state) << " ở cycle " << i; + } + + if (!running) + { + break; + } + fixture.clock_.advance(kControlPeriod); + } +} + +TEST(ControlLoop, NaNFromControllerIsBlockedByArbiter) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kNaN}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); // CONTROLLING, controller trả NaN + + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0); + EXPECT_GT(fixture.loop_.arbiter().nonFiniteRejections(), 0u); +} + +TEST(ControlLoop, OverspeedCommandIsClampedNotPassedThrough) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kTooFast}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); + + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.5); + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().angular.z, 1.0); + EXPECT_GT(fixture.loop_.arbiter().velocityClamps(), 0u); +} + +TEST(ControlLoop, EmptyPlanIsTreatedAsFailureNotAsAValidPlan) +{ + // Fake cố ý vi phạm contract (trả true kèm plan rỗng) để kiểm guard của bên gọi: plan rỗng lọt + // xuống sẽ thành front()/back() trên vector rỗng ở tầng dưới. + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kEmpty}); + + std::string reason; + 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_STREQ(fixture.loop_.lastOutcome(), "FAILED"); +} + +TEST(ControlLoop, LostPoseStopsTheRobotImmediately) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); + ASSERT_GT(fixture.loop_.lastCommand().linear.x, 0.0); + + fixture.pose_.setAvailable(false); + fixture.clock_.advance(kControlPeriod); + 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ũ"; +} + +// ================================================================================================ +// Recovery +// ================================================================================================ + +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. + // Lật kịch bản planner ngay khi quan sát thấy RECOVERING, thay vì đếm trước số cycle — đếm trước + // làm test phụ thuộc vào đúng thời điểm hết kiên nhẫn, thứ không phải là thứ đang được kiểm. + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kFail}); + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached}); + fixture.recovery_.setScript({RecoveryScript::kRunning, RecoveryScript::kSucceeded}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 7), reason)) << reason; + + bool planner_unblocked = false; + std::vector states; + + for (int i = 0; i < 200; ++i) + { + const std::string name = move_base2::toString(fixture.loop_.state()); + if (states.empty() || states.back() != name) + { + states.push_back(name); + } + + if (!planner_unblocked && fixture.loop_.state() == NavigationState::kRecovering) + { + fixture.planner_.setScript({PlannerScript::kOk}); + planner_unblocked = true; + } + + if (!fixture.loop_.step()) + { + states.push_back(move_base2::toString(fixture.loop_.state())); + break; + } + fixture.clock_.advance(kControlPeriod); + } + + EXPECT_TRUE(planner_unblocked) << "không bao giờ vào recovery"; + EXPECT_EQ(states, (std::vector{"IDLE", "PLANNING", "RECOVERING", "PLANNING", + "CONTROLLING", "SUCCEEDED"})) + << join(states); + EXPECT_EQ(fixture.recovery_.startCount(), 1u); + EXPECT_EQ(fixture.recovery_.lastStartIndex(), 0u); + EXPECT_EQ(fixture.recovery_.lastTrigger(), RecoveryTrigger::kPlanningFailed); + EXPECT_EQ(fixture.mission_.reportCountFor(7), 1u); +} + +TEST(ControlLoop, AllRecoveriesExhaustedEndsInAbortedWithSingleReport) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kFail}); // luôn hỏng + fixture.recovery_.setScript({RecoveryScript::kSucceeded}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 9), reason)) << reason; + fixture.run(); + + EXPECT_FALSE(fixture.hitLimit()); + EXPECT_EQ(fixture.states(), + (std::vector{"IDLE", "PLANNING", "RECOVERING", "PLANNING", "RECOVERING", + "PLANNING", "ABORTED"})) + << 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"; + EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED"); + EXPECT_EQ(fixture.loop_.outcomeReportCount(), 1u); + EXPECT_EQ(fixture.mission_.reportCountFor(9), 1u); +} + +TEST(ControlLoop, RecoveryRefusingToStartMovesOnToTheNextBehavior) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kFail}); + fixture.recovery_.setStartSucceeds(false); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + fixture.run(); + + 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"; + EXPECT_STREQ(fixture.loop_.lastOutcome(), "FAILED"); +} + +TEST(ControlLoop, RecoveryVelocityGoesThroughArbiterWithOneHandoverCycle) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kFail}); + fixture.recovery_.setScript({RecoveryScript::kRunning, RecoveryScript::kRunning, + RecoveryScript::kSucceeded}); + fixture.recovery_.setRecoveryVelocity(true, -0.15); // [m/s], âm = lùi + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + bool saw_reverse = false; + for (int i = 0; i < 60; ++i) + { + const bool running = fixture.loop_.step(); + if (fixture.loop_.state() == NavigationState::kRecovering && + 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"; + } + if (!running) + { + break; + } + 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"; +} + +TEST(ControlLoop, RecoveryDisabledAbortsOnFirstFailure) +{ + ControlLoopConfig config = baseConfig(); + config.state_machine.recovery_enabled = false; + config.state_machine.recovery_behavior_count = 0; + + Fixture fixture(config); + fixture.planner_.setScript({PlannerScript::kFail}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + fixture.run(); + + EXPECT_EQ(fixture.recovery_.startCount(), 0u); + EXPECT_EQ(fixture.states(), (std::vector{"IDLE", "PLANNING", "ABORTED"})) + << join(fixture.states()); +} + +// ================================================================================================ +// Pause / cancel +// ================================================================================================ + +TEST(ControlLoop, CancelWhileControllingStopsAndReportsCancelled) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 5), reason)) << reason; + + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + ASSERT_GT(fixture.loop_.lastCommand().linear.x, 0.0); + + fixture.loop_.requestCancel(); + + int cycles_to_zero = 0; + for (int i = 0; i < 10; ++i) + { + fixture.clock_.advance(kControlPeriod); + const bool running = fixture.loop_.step(); + ++cycles_to_zero; + if (!running) + { + break; + } + } + + EXPECT_LE(cycles_to_zero, 2) << "cmd_vel phải về 0 trong vòng 2 cycle sau khi huỷ"; + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0); + EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled); + EXPECT_STREQ(fixture.loop_.lastOutcome(), "CANCELLED"); + EXPECT_EQ(fixture.mission_.reportCountFor(5), 1u); +} + +TEST(ControlLoop, CancelDuringRecoveryStopsTheBehavior) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kFail}); + fixture.recovery_.setScript({RecoveryScript::kRunning}); + fixture.recovery_.setRecoveryVelocity(true, -0.15); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + for (int i = 0; i < 40 && fixture.loop_.state() != NavigationState::kRecovering; ++i) + { + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + } + ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering); + + fixture.loop_.requestCancel(); + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + + EXPECT_EQ(fixture.recovery_.cancelCount(), 1u); + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0); + + fixture.loop_.step(); + EXPECT_EQ(fixture.loop_.state(), NavigationState::kCancelled); +} + +TEST(ControlLoop, PauseAndResumeMidControlling) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + + fixture.loop_.requestPause(); + fixture.clock_.advance(kControlPeriod); + fixture.loop_.step(); + + EXPECT_EQ(fixture.loop_.state(), NavigationState::kPaused); + EXPECT_DOUBLE_EQ(fixture.loop_.lastCommand().linear.x, 0.0); + + // Dừng lâu rồi tiếp tục: không được vì thế mà rơi vào recovery. + fixture.clock_.advance(30.0); + fixture.loop_.requestResume(); + fixture.loop_.step(); + + EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling); + EXPECT_GT(fixture.loop_.lastCommand().linear.x, 0.0); +} + +// ================================================================================================ +// Nhiều chặng liên tiếp +// ================================================================================================ + +TEST(ControlLoop, ThreeSequentialMissionLegsEachReportedExactlyOnce) +{ + Fixture fixture; + + for (std::uint64_t leg = 1; leg <= 3; ++leg) + { + fixture.planner_.setScript({PlannerScript::kOk}); + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(static_cast(leg), leg), reason)) << reason; + + for (int i = 0; i < 50; ++i) + { + const bool running = fixture.loop_.step(); + fixture.clock_.advance(kControlPeriod); + if (!running) + { + break; + } + } + + ASSERT_EQ(fixture.loop_.state(), NavigationState::kSucceeded) << "chặng " << leg; + ASSERT_EQ(fixture.mission_.reportCountFor(leg), 1u) << "chặng " << leg; + } + + EXPECT_EQ(fixture.loop_.outcomeReportCount(), 3u); + EXPECT_EQ(fixture.mission_.reports().size(), 3u); +} + +// ================================================================================================ +// Action của mission (D8) +// ================================================================================================ + +TEST(ControlLoopActions, ActionOnlyMissionNeverTouchesPlannerOrController) +{ + Fixture fixture; + fixture.action_.setScript({ActionScript::kRunning, ActionScript::kSucceeded, + ActionScript::kRunning, ActionScript::kSucceeded}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeActionOnlyRequest(2, 7), reason)) << reason; + fixture.run(); + + ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states()); + EXPECT_EQ(fixture.states(), + (std::vector{"IDLE", "EXECUTING_ACTIONS", "SUCCEEDED"})) + << join(fixture.states()); + + // Không một lời gọi nào tới planner/controller — và vì thế không một lệnh vận tốc nào khác 0. + EXPECT_EQ(fixture.planner_.makePlanCount(), 0u); + EXPECT_EQ(fixture.controller_.computeCount(), 0u); + EXPECT_EQ(fixture.loop_.lastCommand().linear.x, 0.0); + EXPECT_EQ(fixture.loop_.lastCommand().angular.z, 0.0); + + // Hai action tới port nguyên vẹn, đúng thứ tự; kết quả báo đúng một lần sau action cuối. + EXPECT_EQ(fixture.action_.startedActionTypes(), + (std::vector{"act_0", "act_1"})); + EXPECT_EQ(fixture.mission_.reportCountFor(7), 1u); +} + +TEST(ControlLoopActions, MissionWithGoalRunsActionsAfterGoalReached) +{ + Fixture fixture; + fixture.planner_.setScript({PlannerScript::kOk}); + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kGoalReached}); + fixture.action_.setScript({ActionScript::kRunning, ActionScript::kSucceeded}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(withActions(makeRequest(2.0, 11), 1), reason)) << reason; + fixture.run(); + + ASSERT_FALSE(fixture.hitLimit()) << join(fixture.states()); + EXPECT_EQ(fixture.states(), + (std::vector{"IDLE", "PLANNING", "CONTROLLING", "EXECUTING_ACTIONS", + "SUCCEEDED"})) + << join(fixture.states()); + EXPECT_EQ(fixture.action_.startedActionTypes(), (std::vector{"act_0"})); + EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u); +} + +TEST(ControlLoopActions, ActionFailureAbortsAndReportsOnce) +{ + Fixture fixture; + fixture.action_.setScript({ActionScript::kRunning, ActionScript::kFailed}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeActionOnlyRequest(1, 13), reason)) << reason; + fixture.run(); + + 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"; +} + +TEST(ControlLoopActions, SubmitRejectsActionRequestWhenActionPortMissing) +{ + Fixture fixture; + + ControlLoop bare_loop; + ControlLoopDeps deps = fixture.deps_; + deps.action = nullptr; + + std::string error; + ASSERT_TRUE(bare_loop.configure(baseConfig(), deps, error)) << error; + + std::string reason; + EXPECT_FALSE(bare_loop.submit(makeActionOnlyRequest(1), reason)); + EXPECT_NE(reason.find("action port"), std::string::npos) << reason; + + // 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; +} + +// ================================================================================================ +// Lập plan bất đồng bộ (kBusy) +// +// Global planner nặng mất hàng trăm ms, control loop chạy 20 Hz và là thread duy nhất phát cmd_vel. +// Các test dưới đây khoá lại điều kiện để tách được hai nhịp đó mà state machine không phải đoán. +// ================================================================================================ + +TEST(ControlLoopAsyncPlanner, StaysInPlanningWhileThePlannerIsStillWorking) +{ + Fixture fixture; + fixture.planner_.setLatencyCycles(3); + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.stepOnce(); // IDLE -> PLANNING, kick lượt lập plan + ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning); + + for (int i = 0; i < 3; ++i) + { + fixture.stepOnce(); + EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning) + << "rời PLANNING khi planner còn đang tính, cycle " << i; + } + + fixture.stepOnce(); + EXPECT_EQ(fixture.loop_.state(), NavigationState::kControlling); +} + +TEST(ControlLoopAsyncPlanner, DoesNotRestartAPlanThatIsAlreadyRunning) +{ + // `start_planner` là tín hiệu MỨC, bật lại mỗi cycle chừng nào còn ở PLANNING. Kick lại một lượt + // đang chạy sẽ làm mất toàn bộ thời gian đã bỏ ra cho nó. + Fixture fixture; + fixture.planner_.setLatencyCycles(4); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + for (int i = 0; i < 4; ++i) + { + fixture.stepOnce(); + } + + EXPECT_EQ(fixture.planner_.makePlanCount(), 1u) + << "lượt lập plan bị khởi động lại mỗi cycle"; +} + +TEST(ControlLoopAsyncPlanner, PlannerPatienceStillFiresWhileThePlannerIsBusy) +{ + // Đây là thứ DUY NHẤT phát hiện được thread planner treo. Ở chế độ đồng bộ, planner treo làm + // treo luôn control loop nên còn dễ thấy; ở chế độ bất đồng bộ nó im lặng hoàn toàn. + ControlLoopConfig config = baseConfig(); + config.state_machine.planner_patience = 0.2; // [s] = 4 cycle + config.state_machine.max_planning_retries = -1; + + Fixture fixture(config); + fixture.planner_.setLatencyCycles(1000); // không bao giờ xong + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + for (int i = 0; i < 20; ++i) + { + fixture.stepOnce(); + } + + // Planner treo vĩnh viễn thì đường đi đúng là: hết kiên nhẫn -> recovery -> thử lại -> hết + // 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: " + << join(fixture.states()); + EXPECT_EQ(fixture.loop_.state(), NavigationState::kAborted) << join(fixture.states()); +} + +TEST(ControlLoopAsyncPlanner, KeepsFollowingTheOldPlanWhileReplanningInBackground) +{ + // Chính là lý do tồn tại của cả mô hình: lập lại plan không được làm gián đoạn việc bám plan cũ. + ControlLoopConfig config = baseConfig(); + config.state_machine.controller_patience = 100.0; // [s] không cho controller hết kiên nhẫn + Fixture fixture(config); + + fixture.controller_.setNominalSpeed(0.3); // [m/s] + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.stepOnce(); // PLANNING + fixture.stepOnce(); // -> CONTROLLING + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + + for (int i = 0; i < 5; ++i) + { + 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; + } +} + +TEST(ControlLoopAsyncPlanner, CancelWhilePlanningCancelsTheRunningPlan) +{ + // Không huỷ thì lượt đang bay vẫn về và chiếm chỗ hộp thư, rồi bị nhận nhầm cho yêu cầu kế tiếp. + Fixture fixture; + fixture.planner_.setLatencyCycles(100); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.stepOnce(); + ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning); + ASSERT_TRUE(fixture.planner_.isPlanning()); + + fixture.loop_.requestCancel(); + fixture.stepOnce(); + + EXPECT_GE(fixture.planner_.cancelCount(), 1u); + EXPECT_FALSE(fixture.planner_.isPlanning()); +} + +TEST(ControlLoopAsyncPlanner, AcceptingANewRequestCancelsAnInFlightPlan) +{ + Fixture fixture; + fixture.planner_.setLatencyCycles(100); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + fixture.stepOnce(); + ASSERT_TRUE(fixture.planner_.isPlanning()); + + const std::size_t before = fixture.planner_.cancelCount(); + fixture.loop_.requestCancel(); + fixture.stepOnce(); + fixture.stepOnce(); + + ASSERT_TRUE(fixture.loop_.submit(makeRequest(5.0), reason)) << reason; + 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ũ"; +} + +TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt) +{ + // Không khởi động được mà im lặng thì state machine đứng ở PLANNING vô hạn: không có kFailed để + // đếm lượt, không có kPlanReady để đi tiếp. + ControlLoopConfig config = baseConfig(); + config.state_machine.max_planning_retries = 2; + Fixture fixture(config); + fixture.planner_.setSwapSucceeds(true); + fixture.planner_.setScript({PlannerScript::kFail}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + + fixture.run(); + + EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states()); + EXPECT_NE(std::find(fixture.states().begin(), fixture.states().end(), "RECOVERING"), + fixture.states().end()) + << join(fixture.states()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}