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

first commit

Browse Source
This commit is contained in:
duongtd
2026-07-29 15:45:16 +07:00
commit 4762a3032c
56 changed files with 15310 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

492
CMakeLists.txt Normal file
View File

@@ -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/<pkg> -> src/AMR_T800/pnkx_nav_core/src
set(PNKX_NAV_CORE_SRC_DIR
"${CMAKE_CURRENT_SOURCE_DIR}/../../pnkx_nav_core/src"
)
# Test/<pkg> -> 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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# ========================================================
# ActionHandler mặc định — plugin riêng, nạp qua Boost.DLL như mọi handler khác.
# ========================================================
add_library(move_base2_noop_action_handler SHARED
plugins/noop_action_handler.cpp
)
target_compile_options(move_base2_noop_action_handler PRIVATE -Wall -Wextra)
target_include_directories(move_base2_noop_action_handler
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
set_target_properties(move_base2_noop_action_handler PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_link_libraries(move_base2_noop_action_handler
PUBLIC
move_base2_core
PRIVATE
${catkin_LIBRARIES}
Boost::boost
Boost::system
Boost::filesystem
${CMAKE_DL_LIBS}
)
# ========================================================
# Plugin library — facade BaseNavigation + export Boost.DLL.
# ========================================================
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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/<pkg>/<test>).
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()

84
README.md Normal file
View File

@@ -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`.

122
docs/ARCHITECTURE.md Normal file
View File

@@ -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).

119
docs/STATE_MACHINE.md Normal file
View File

@@ -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.

72
docs/THREADING.md Normal file
View File

@@ -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.

142
include/move_base2/bridges/mission_adapter_bridge.h Normal file
View File

@@ -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 <cstddef>
#include <cstdint>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <mission_adapters/navigation_client.h>
#include <mission_adapters/types.h>
#include <move_base2/ports/mission_port.h>
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<void()>;
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<const mission_adapters::Mission>& 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<const mission_adapters::Mission> 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_

153
include/move_base2/config/move_base2_config.h Normal file
View File

@@ -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 <string>
#include <robot/node_handle.h>
#include <move_base2/control_loop.h>
#include <move_base2/io/sensor_gateway.h>
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 (`<ns>/behaviors`) trong YAML.
std::string recovery_namespace = "recovery";
/// Namespace chứa danh sách action handler (`<ns>/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` -> `<profile>.local_planner_name`;
* - `<TênPlanner>: base_global_planner` -> `<profile>.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_

292
include/move_base2/control_loop.h Normal file
View File

@@ -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 <cstddef>
#include <cstdint>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <move_base2/core/navigation_request.h>
#include <move_base2/core/state_machine.h>
#include <move_base2/core/velocity_arbiter.h>
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/planner_port.h>
#include <move_base2/ports/pose_port.h>
#include <move_base2/ports/recovery_port.h>
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<robot_geometry_msgs::PoseStamped> 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_

114
include/move_base2/core/navigation_request.h Normal file
View File

@@ -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 <cstdint>
#include <memory>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_protocol_msgs/Action.h>
#include <robot_protocol_msgs/Order.h>
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<robot_protocol_msgs::Action> 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<robot_protocol_msgs::Order> 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_

61
include/move_base2/core/navigation_state.h Normal file
View File

@@ -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_

327
include/move_base2/core/state_machine.h Normal file
View File

@@ -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 <cstddef>
#include <string>
#include <robot/time.h>
#include <move_base2/core/navigation_state.h>
#include <move_base2/core/velocity_arbiter.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/recovery_port.h>
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_

180
include/move_base2/core/velocity_arbiter.h Normal file
View File

@@ -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 <cstddef>
#include <string>
#include <robot_geometry_msgs/Twist.h>
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_

206
include/move_base2/io/sensor_gateway.h Normal file
View File

@@ -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 <cstddef>
#include <memory>
#include <string>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
// 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<T>` 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_filter::LaserScanSOR> laser_sor_;
SensorGatewayStats stats_;
};
} // namespace move_base2
#endif // MOVE_BASE2_IO_SENSOR_GATEWAY_H_

241
include/move_base2/navigation_server.h Normal file
View File

@@ -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 <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
#include <move_base_core/navigation.h>
#include <move_base2/control_loop.h>
#include <move_base2/core/navigation_request.h>
#include <move_base2/io/sensor_gateway.h>
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<robot_geometry_msgs::Point>& fprt) override;
std::vector<robot_geometry_msgs::Point> 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<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() override;
std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() override;
std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() override;
std::map<std::string, robot_sensor_msgs::PointCloud2> 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<robot_geometry_msgs::Point> footprint_;
std::map<std::string, robot_sensor_msgs::DepthCameraData::ConstPtr> depth_camera_data_;
/// Frame đóng dấu lên lệnh vận tốc gửi host. Chép từ config lúc @ref configureLoop.
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_

92
include/move_base2/ports/action_port.h Normal file
View File

@@ -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 <string>
// 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 <boost/shared_ptr.hpp>
#include <robot_protocol_msgs/Action.h>
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_

46
include/move_base2/ports/clock_port.h Normal file
View File

@@ -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 <robot/time.h>
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_

97
include/move_base2/ports/controller_port.h Normal file
View File

@@ -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 <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/Vector3.h>
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<robot_geometry_msgs::PoseStamped>& 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_

70
include/move_base2/ports/mission_port.h Normal file
View File

@@ -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 <cstdint>
#include <functional>
#include <move_base2/core/navigation_request.h>
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<void(const NavigationRequest&)>;
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_

109
include/move_base2/ports/planner_port.h Normal file
View File

@@ -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 <cstdint>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_protocol_msgs/Order.h>
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<robot_geometry_msgs::PoseStamped> 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_

38
include/move_base2/ports/pose_port.h Normal file
View File

@@ -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 <robot_geometry_msgs/PoseStamped.h>
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_

135
include/move_base2/ports/recovery_port.h Normal file
View File

@@ -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 <cstddef>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
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<robot_geometry_msgs::PoseStamped> 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_

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

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

112
include/move_base2/runners/action_runner.h Normal file
View File

@@ -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 <cstddef>
#include <functional>
#include <map>
#include <string>
#include <vector>
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/runners/action_handler.h>
namespace move_base2
{
/**
* @class ActionRunner
* @brief Bảng tra `actionType` -> handler, nạp từ YAML bằng Boost.DLL.
*
* 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 `<ns>/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<std::string> 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<ActionHandler::Ptr> handlers_;
std::map<std::string, ActionHandler*> 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<std::function<ActionHandler::Ptr()>> factories_;
};
} // namespace move_base2
#endif // MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_

157
include/move_base2/runners/controller_runner.h Normal file
View File

@@ -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 <cstddef>
#include <functional>
#include <map>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <robot_nav_core/base_local_planner.h>
#include <move_base2/ports/controller_port.h>
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<robot_geometry_msgs::PoseStamped>& 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<robot_nav_core::BaseLocalPlanner::Ptr()> 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<std::string, Loaded> 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_

176
include/move_base2/runners/planner_runner.h Normal file
View File

@@ -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 <condition_variable>
#include <cstddef>
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <robot/node_handle.h>
#include <robot_nav_core/base_global_planner.h>
#include <move_base2/ports/planner_port.h>
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<robot_nav_core::BaseGlobalPlanner::Ptr()> 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<std::string, Loaded> 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<robot_protocol_msgs::Order> request_order_;
std::uint64_t request_tag_ = 0;
std::vector<robot_geometry_msgs::PoseStamped> planning_; ///< Thread ghi vào.
std::vector<robot_geometry_msgs::PoseStamped> handoff_; ///< Hộp thư.
std::uint64_t result_tag_ = 0;
bool result_ok_ = false;
};
} // namespace move_base2
#endif // MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_

164
include/move_base2/runners/recovery_runner.h Normal file
View File

@@ -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 <cstddef>
#include <functional>
#include <string>
#include <vector>
#include <recovery_core/adapters/costmap_collision_checker.h>
#include <recovery_core/recovery_registry.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/pose_port.h>
#include <move_base2/ports/recovery_port.h>
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<bool(std::vector<robot_geometry_msgs::PoseStamped>&)>;
/**
* @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 `<ns>/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<robot_geometry_msgs::PoseStamped>& 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_

84
package.xml Normal file
View File

@@ -0,0 +1,84 @@
<package>
<name>move_base2</name>
<version>0.1.0</version>
<description>
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.
</description>
<author>DuongTD</author>
<maintainer email="xroboticdevs@gmail.com">DuongTD</maintainer>
<license>BSD</license>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>move_base_core</build_depend>
<run_depend>move_base_core</run_depend>
<build_depend>robot_nav_core</build_depend>
<run_depend>robot_nav_core</run_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
<build_depend>robot_time</build_depend>
<run_depend>robot_time</run_depend>
<build_depend>robot_geometry_msgs</build_depend>
<run_depend>robot_geometry_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>robot_nav_msgs</build_depend>
<run_depend>robot_nav_msgs</run_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<build_depend>robot_sensor_msgs</build_depend>
<run_depend>robot_sensor_msgs</run_depend>
<build_depend>robot_map_msgs</build_depend>
<run_depend>robot_map_msgs</run_depend>
<build_depend>robot_protocol_msgs</build_depend>
<run_depend>robot_protocol_msgs</run_depend>
<build_depend>robot_xmlrpcpp</build_depend>
<run_depend>robot_xmlrpcpp</run_depend>
<build_depend>tf3</build_depend>
<run_depend>tf3</run_depend>
<build_depend>yaml-cpp</build_depend>
<run_depend>yaml-cpp</run_depend>
<!-- Chỉ dùng cho test: bộ fake và scenario runner dùng chung. -->
<test_depend>nav_test_harness</test_depend>
<build_depend>recovery_core</build_depend>
<run_depend>recovery_core</run_depend>
<build_depend>laser_filter</build_depend>
<run_depend>laser_filter</run_depend>
<build_depend>mission_adapters</build_depend>
<run_depend>mission_adapters</run_depend>
</package>

171
plugins/noop_action_handler.cpp Normal file
View File

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

239
src/bridges/mission_adapter_bridge.cpp Normal file
View File

@@ -0,0 +1,239 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt MissionAdapterBridge.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/bridges/mission_adapter_bridge.h>
#include <utility>
#include <mission_adapters/mission_manager.h>
#include <robot/robot.h>
namespace move_base2
{
MissionAdapterBridge::MissionAdapterBridge() = default;
MissionAdapterBridge::~MissionAdapterBridge() = default;
void MissionAdapterBridge::attach(mission_adapters::MissionManager* manager)
{
std::lock_guard<std::mutex> lock(mutex_);
manager_ = manager;
}
void MissionAdapterBridge::setCancelCallback(CancelCallback callback)
{
std::lock_guard<std::mutex> lock(mutex_);
cancel_callback_ = std::move(callback);
}
void MissionAdapterBridge::setRequestCallback(RequestCallback callback)
{
std::lock_guard<std::mutex> 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<const mission_adapters::Mission>& mission)
{
if (!mission)
{
robot::log_error("[move_base2] MissionAdapterBridge: dispatch(nullptr).\n");
return false;
}
std::lock_guard<std::mutex> 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<unsigned long long>(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<unsigned long long>(mission->id),
static_cast<unsigned long long>(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<std::mutex> 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<const mission_adapters::Mission> mission;
RequestCallback callback;
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
++stale_outcomes_;
}
}
bool MissionAdapterBridge::hasActiveMission() const
{
mission_adapters::MissionManager* manager = nullptr;
bool has_pending = false;
{
std::lock_guard<std::mutex> lock(mutex_);
manager = manager_;
has_pending = static_cast<bool>(pending_);
}
if (has_pending)
{
return true;
}
return manager != nullptr && manager->hasMission();
}
void MissionAdapterBridge::start()
{
std::lock_guard<std::mutex> lock(mutex_);
running_ = true;
}
void MissionAdapterBridge::stop()
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
return dropped_requests_;
}
std::size_t MissionAdapterBridge::staleOutcomes() const
{
std::lock_guard<std::mutex> lock(mutex_);
return stale_outcomes_;
}
} // namespace move_base2

261
src/config/move_base2_config.cpp Normal file
View File

@@ -0,0 +1,261 @@
/*********************************************************************
* move_base2 — đọc và validate cấu hình runtime.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/config/move_base2_config.h>
#include <cmath>
#include <sstream>
#include <robot/robot.h>
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

569
src/control_loop.cpp Normal file
View File

@@ -0,0 +1,569 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt control loop.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/control_loop.h>
#include <cmath>
#include <sstream>
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

337
src/io/sensor_gateway.cpp Normal file
View File

@@ -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 <move_base2/io/sensor_gateway.h>
#include <exception>
#include <sstream>
#include <laser_filter/laser_filter.h>
#include <robot/console.h>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
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 <typename T>
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<T>(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

35
src/move_base2_plugin.cpp Normal file
View File

@@ -0,0 +1,35 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — export plugin Boost.DLL.
*
* Author: DuongTD
*********************************************************************/
#include <memory>
#include <boost/dll/alias.hpp>
#include <move_base_core/navigation.h>
#include <move_base2/navigation_server.h>
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<NavigationServer>();
}
} // namespace move_base2
BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase2)

653
src/navigation_server.cpp Normal file
View File

@@ -0,0 +1,653 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt facade BaseNavigation.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/navigation_server.h>
#include <cmath>
#include <robot_nav_2d_utils/conversions.h>
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>
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<robot::move_base_core::NavFeedback>();
nav_feedback_->navigation_state = robot::move_base_core::State::PENDING;
nav_feedback_->feed_back_str = "chưa khởi tạo";
nav_feedback_->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<std::string, robot_nav_msgs::OccupancyGrid> maps;
{
std::lock_guard<std::mutex> 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<std::mutex> 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<robot_geometry_msgs::Point>& fprt)
{
std::lock_guard<std::mutex> lock(data_mutex_);
footprint_ = fprt;
}
std::vector<robot_geometry_msgs::Point> NavigationServer::getRobotFootprint()
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(data_mutex_);
return lookupOrDefault(static_maps_, map_name);
}
robot_sensor_msgs::LaserScan NavigationServer::getLaserScan(const std::string& laser_scan_name)
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> lock(data_mutex_);
return lookupOrDefault(point_cloud2s_, point_cloud2_name);
}
std::map<std::string, robot_nav_msgs::OccupancyGrid> NavigationServer::getAllStaticMaps()
{
std::lock_guard<std::mutex> lock(data_mutex_);
return static_maps_;
}
std::map<std::string, robot_sensor_msgs::LaserScan> NavigationServer::getAllLaserScans()
{
std::lock_guard<std::mutex> lock(data_mutex_);
return laser_scans_;
}
std::map<std::string, robot_sensor_msgs::PointCloud> NavigationServer::getAllPointClouds()
{
std::lock_guard<std::mutex> lock(data_mutex_);
return point_clouds_;
}
std::map<std::string, robot_sensor_msgs::PointCloud2> NavigationServer::getAllPointCloud2s()
{
std::lock_guard<std::mutex> lock(data_mutex_);
return point_cloud2s_;
}
bool NavigationServer::removeStaticMap(const std::string& map_name)
{
std::lock_guard<std::mutex> lock(data_mutex_);
return static_maps_.erase(map_name) > 0;
}
bool NavigationServer::removeLaserScan(const std::string& laser_scan_name)
{
std::lock_guard<std::mutex> lock(data_mutex_);
return laser_scans_.erase(laser_scan_name) > 0;
}
bool NavigationServer::removePointCloud(const std::string& point_cloud_name)
{
std::lock_guard<std::mutex> lock(data_mutex_);
return point_clouds_.erase(point_cloud_name) > 0;
}
bool NavigationServer::removePointCloud2(const std::string& point_cloud2_name)
{
std::lock_guard<std::mutex> lock(data_mutex_);
return point_cloud2s_.erase(point_cloud2_name) > 0;
}
bool NavigationServer::removeAllStaticMaps()
{
std::lock_guard<std::mutex> lock(data_mutex_);
static_maps_.clear();
return true;
}
bool NavigationServer::removeAllLaserScans()
{
std::lock_guard<std::mutex> lock(data_mutex_);
laser_scans_.clear();
return true;
}
bool NavigationServer::removeAllPointClouds()
{
std::lock_guard<std::mutex> lock(data_mutex_);
point_clouds_.clear();
return true;
}
bool NavigationServer::removeAllPointCloud2s()
{
std::lock_guard<std::mutex> lock(data_mutex_);
point_cloud2s_.clear();
return true;
}
bool NavigationServer::removeAllData()
{
std::lock_guard<std::mutex> 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<robot_protocol_msgs::Order>(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<robot_protocol_msgs::Order>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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

135
src/navigation_state.cpp Normal file
View File

@@ -0,0 +1,135 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — tên state và phân loại state.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/core/navigation_state.h>
#include <move_base2/core/navigation_request.h>
#include <move_base2/core/velocity_arbiter.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/recovery_port.h>
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

302
src/runners/action_runner.cpp Normal file
View File

@@ -0,0 +1,302 @@
/*********************************************************************
* move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/runners/action_runner.h>
#include <utility>
#include <boost/dll/import.hpp>
#include <boost/system/system_error.hpp>
#include <yaml-cpp/yaml.h>
#include <robot/robot.h>
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<std::string> 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<std::string> ActionRunner::supportedActionTypes() const
{
std::vector<std::string> types;
types.reserve(by_type_.size());
for (const auto& entry : by_type_)
{
types.push_back(entry.first);
}
return types;
}
bool ActionRunner::loadOne(const std::string& name, const std::string& type,
robot::NodeHandle& nh, const std::string& ns)
{
robot::PluginLoaderHelper loader(nh);
const std::string library_path = loader.findLibraryPath(type);
if (library_path.empty())
{
robot::log_error("[move_base2] ActionRunner: không tìm được thư viện cho '%s' — kiểm khoá "
"'%s/library_path' trong YAML và sự tồn tại của file .so.",
type.c_str(), type.c_str());
return false;
}
std::function<ActionHandler::Ptr()> factory;
try
{
factory = boost::dll::import_alias<ActionHandler::Ptr()>(
library_path, type, boost::dll::load_mode::append_decorations);
}
catch (const boost::system::system_error& ex)
{
robot::log_error("[move_base2] ActionRunner: không nạp được symbol '%s' từ '%s': %s",
type.c_str(), library_path.c_str(), ex.what());
return false;
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ActionRunner: lỗi khi nạp '%s': %s", type.c_str(), ex.what());
return false;
}
ActionHandler::Ptr handler;
try
{
handler = factory();
}
catch (const std::exception& ex)
{
robot::log_error("[move_base2] ActionRunner: factory của '%s' ném exception: %s", type.c_str(),
ex.what());
return false;
}
if (!handler)
{
robot::log_error("[move_base2] ActionRunner: factory của '%s' trả về null.", type.c_str());
return false;
}
const std::string param_ns = ns.empty() ? name : ns + "/" + name;
robot::NodeHandle handler_nh(nh, param_ns);
if (!handler->configure(name, handler_nh))
{
robot::log_error("[move_base2] ActionRunner: '%s' (instance '%s') configure() thất bại.",
type.c_str(), name.c_str());
return false;
}
if (!registerHandler(handler))
{
return false;
}
factories_.push_back(std::move(factory));
robot::log_info("[move_base2] ActionRunner: nạp '%s' (instance '%s').", type.c_str(),
name.c_str());
return true;
}
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<std::string>();
name = entry["name"] ? entry["name"].as<std::string>() : type;
}
catch (const YAML::Exception& ex)
{
robot::log_error("[move_base2] ActionRunner: '%s[%zu]' không đọc được: %s", key.c_str(), i,
ex.what());
all_ok = false;
continue;
}
if (!loadOne(name, type, nh, namespace_))
{
all_ok = false;
}
}
configured_ = true;
return all_ok;
}
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

408
src/runners/controller_runner.cpp Normal file
View File

@@ -0,0 +1,408 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt ControllerRunner.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/runners/controller_runner.h>
#include <cmath>
#include <exception>
#include <utility>
#include <boost/dll/import.hpp>
#include <boost/system/system_error.hpp>
#include <robot/plugin_loader_helper.h>
#include <robot/robot.h>
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<robot_nav_core::BaseLocalPlanner::Ptr()>(
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<robot_geometry_msgs::PoseStamped>& 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

395
src/runners/planner_runner.cpp Normal file
View File

@@ -0,0 +1,395 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt PlannerRunner.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/runners/planner_runner.h>
#include <cmath>
#include <exception>
#include <utility>
#include <boost/dll/import.hpp>
#include <boost/system/system_error.hpp>
#include <robot/plugin_loader_helper.h>
#include <robot/robot.h>
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<std::mutex> 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<robot_nav_core::BaseGlobalPlanner::Ptr()>(
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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<robot_protocol_msgs::Order>(*order)
: nullptr;
request_tag_ = tag;
pending_ = true;
discard_ = false;
has_result_ = false;
cv_.notify_one();
return true;
}
bool PlannerRunner::isPlanning() const
{
std::lock_guard<std::mutex> lock(mutex_);
return pending_ || running_;
}
bool PlannerRunner::pollPlan(PlanResult& result)
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<robot_protocol_msgs::Order> 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

253
src/runners/recovery_runner.cpp Normal file
View File

@@ -0,0 +1,253 @@
/*********************************************************************
* move_base2 — hiện thực RecoveryPort bằng recovery_core.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/runners/recovery_runner.h>
#include <utility>
#include <robot/robot.h>
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

578
src/state_machine.cpp Normal file
View File

@@ -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 <move_base2/core/state_machine.h>
#include <sstream>
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

245
src/velocity_arbiter.cpp Normal file
View File

@@ -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 <move_base2/core/velocity_arbiter.h>
#include <algorithm>
#include <cmath>
#include <sstream>
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

363
test/action_runner_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cstdlib>
#include <memory>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <move_base2/runners/action_runner.h>
#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<std::string> types) : types_(std::move(types))
{
}
bool configure(const std::string& name, robot::NodeHandle& /*nh*/) override
{
name_ = name;
return configure_ok;
}
std::vector<std::string> 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<std::string> 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<ScriptedHandler>(std::vector<std::string>{"pick"});
auto drop = std::make_shared<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"pick"})));
}
TEST(ActionRunner, RejectsHandlerWithoutTypes)
{
Rig rig;
EXPECT_FALSE(rig.runner.registerHandler(std::make_shared<ScriptedHandler>(
std::vector<std::string>{})));
EXPECT_FALSE(rig.runner.registerHandler(nullptr));
}
TEST(ActionRunner, UnknownActionTypeFailsStart)
{
Rig rig;
ASSERT_TRUE(rig.runner.registerHandler(
std::make_shared<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"pick"})));
EXPECT_FALSE(rig.runner.start(makeAction("pick")));
}
TEST(ActionRunner, HandlerRefusingStartIsReported)
{
Rig rig;
auto handler = std::make_shared<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"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<ScriptedHandler>(std::vector<std::string>{"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();
}

171
test/config/move_base2_params.yaml Normal file
View File

@@ -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.

245
test/config_validation_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cstdlib>
#include <string>
#include <robot/node_handle.h>
#include <move_base2/config/move_base2_config.h>
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();
}

381
test/controller_runner_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cmath>
#include <cstdint>
#include <cstdlib>
#include <limits>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <move_base2/runners/controller_runner.h>
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<robot_costmap_2d::Costmap2DROBOT*>(&placeholder);
}
std::vector<robot_geometry_msgs::PoseStamped> makePlan(std::size_t poses = 3)
{
std::vector<robot_geometry_msgs::PoseStamped> 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<double>(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<double>::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<double>::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();
}

870
test/fake_ports.h Normal file
View File

@@ -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 <cstddef>
#include <cstdint>
#include <map>
#include <limits>
#include <string>
#include <utility>
#include <vector>
#include <robot/time.h>
#include <move_base2/ports/action_port.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/mission_port.h>
#include <move_base2/ports/planner_port.h>
#include <move_base2/ports/pose_port.h>
#include <move_base2/ports/recovery_port.h>
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<PlannerScript> 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<PlannerScript> 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<robot_geometry_msgs::PoseStamped>& 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<double>::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<ControllerScript> 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<ControllerScript> 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<RecoveryScript> 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<std::size_t>& 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<RecoveryScript> 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<std::size_t, RecoveryOutputKind> 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<std::size_t> 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<ActionScript> 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<std::string>& 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<ActionScript> 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<std::string> 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<std::pair<std::uint64_t, NavigationOutcome>>& 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<std::pair<std::uint64_t, NavigationOutcome>> reports_;
bool active_ = false;
};
} // namespace testing
} // namespace move_base2
#endif // MOVE_BASE2_TEST_FAKE_PORTS_H_

559
test/navigation_server_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cmath>
#include <cstddef>
#include <limits>
#include <string>
#include <vector>
#include <robot_costmap_2d/layered_costmap.h>
#include <move_base2/navigation_server.h>
#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<float>(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<float> 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<const robot_sensor_msgs::LaserScan*>(data)->ranges;
}
});
fixture.attachCostmaps();
fixture.server_.addLaserScan("/b_scan", makeScan());
const std::vector<float> 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<robot_sensor_msgs::DepthCameraData>();
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<double>::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();
}

422
test/planner_runner_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <chrono>
#include <cstdlib>
#include <cstdint>
#include <limits>
#include <string>
#include <thread>
#include <vector>
#include <robot/node_handle.h>
#include <move_base2/runners/planner_runner.h>
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<robot_costmap_2d::Costmap2DROBOT*>(&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<double>::quiet_NaN();
const double inf = std::numeric_limits<double>::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();
}

128
test/plugins/test_global_planner.cpp Normal file
View File

@@ -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 <stdexcept>
#include <string>
#include <vector>
#include <boost/config.hpp>
#include <boost/dll/alias.hpp>
#include <boost/make_shared.hpp>
#include <robot_nav_core/base_global_planner.h>
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<robot_geometry_msgs::PoseStamped>& 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<robot_geometry_msgs::PoseStamped>& 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>(TestGlobalPlanner::Behavior::kOk);
}
robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan()
{
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kEmptyPlan);
}
robot_nav_core::BaseGlobalPlanner::Ptr createThrowing()
{
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kThrow);
}
robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing()
{
return std::make_shared<TestGlobalPlanner>(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)

216
test/plugins/test_local_planner.cpp Normal file
View File

@@ -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 <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include <boost/config.hpp>
#include <boost/dll/alias.hpp>
#include <robot_nav_core/base_local_planner.h>
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<robot_geometry_msgs::PoseStamped>& plan) override
{
return !plan.empty();
}
void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
{
path.clear();
}
void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& 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<double>::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>(TestLocalPlanner::Behavior::kOk);
}
robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}
robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
}
robot_nav_core::BaseLocalPlanner::Ptr createNaN()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
}
robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
}
robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
{
return std::make_shared<TestLocalPlanner>(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)

237
test/recovery_runner_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cstdlib>
#include <string>
#include <robot/node_handle.h>
#include <move_base2/runners/recovery_runner.h>
#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();
}

393
test/sensor_gateway_test.cpp Normal file
View File

@@ -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<T>` 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 <gtest/gtest.h>
#include <stdexcept>
#include <string>
#include <typeinfo>
#include <vector>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <move_base2/io/sensor_gateway.h>
#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<float>(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<robot_sensor_msgs::DepthCameraData>();
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();
}

116
test/spy_layer.h Normal file
View File

@@ -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 <cstddef>
#include <functional>
#include <stdexcept>
#include <string>
#include <typeinfo>
#include <vector>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>
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<void(const void*, const std::type_info&, const std::string&)>;
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<Record>& 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<Record> records_;
};
using SpyPtr = boost::shared_ptr<SpyLayer>;
/// @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<SpyLayer>(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_

1257
test/state_machine_test.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

398
test/velocity_arbiter_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <cmath>
#include <limits>
#include <string>
#include <move_base2/core/velocity_arbiter.h>
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<double>::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<double>::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<double>::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();
}

1009
test/walking_skeleton_test.cpp Normal file
View File

File diff suppressed because it is too large Load Diff