From d8babff20b0a39a98653d10bea41efa787ee814e Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 30 Jul 2026 09:24:47 +0700 Subject: [PATCH] optimal --- CMakeLists.txt | 12 + config/runtime/config | 1 + config/runtime/costmap_common_params.yaml | 1 + config/runtime/costmap_global_params.yaml | 1 + ...lobal_params_plugins_no_virtual_walls.yaml | 1 + config/runtime/costmap_local_params.yaml | 1 + ...local_params_plugins_no_virtual_walls.yaml | 1 + config/runtime/custom_global_params.yaml | 1 + config/runtime/dock_global_params.yaml | 1 + .../runtime/hybrid_local_planner_params.yaml | 1 + config/runtime/maker_sources.yaml | 1 + config/runtime/mission_adapters_params.yaml | 1 + config/runtime/move_base_common_params.yaml | 60 ++++ config/runtime/mppi_local_planner_params.yaml | 1 + config/runtime/mprim | 1 + config/runtime/pnkx_local_planner_params.yaml | 1 + .../runtime/priest_local_planner_params.yaml | 1 + config/runtime/recovery_behaviors_params.yaml | 1 + config/runtime/sbpl_global_params.yaml | 1 + .../runtime/stanley_local_planner_params.yaml | 1 + config/runtime/two_points_global_params.yaml | 1 + include/move_base2/control_loop.h | 12 + include/move_base2/core/state_machine.h | 20 ++ include/move_base2/io/costmap_exporter.h | 91 +++++ include/move_base2/navigation_runtime.h | 229 ++++++++++++ include/move_base2/navigation_server.h | 100 +++++- include/move_base2/ports/controller_port.h | 12 + .../move_base2/runners/controller_runner.h | 55 ++- launch/README.md | 110 ++++++ launch/move_base2_control.launch | 66 ++++ launch/move_base2_sim.launch | 54 +++ package.xml | 3 +- src/config/move_base2_config.cpp | 182 ++++++++++ src/control_loop.cpp | 68 +++- src/io/costmap_exporter.cpp | 148 ++++++++ src/move_base2_plugin.cpp | 13 + src/navigation_runtime.cpp | 202 +++++++++++ src/navigation_server.cpp | 329 +++++++++++++++++- src/runners/controller_runner.cpp | 137 ++++++-- src/state_machine.cpp | 126 +++++-- test/config/move_base2_params.yaml | 24 ++ test/config_validation_test.cpp | 99 ++++++ test/controller_runner_test.cpp | 40 ++- test/fake_ports.h | 12 + test/mission_adapter_bridge_test.cpp | 287 +++++++++++++++ test/move_base2_scenario_driver.h | 294 ++++++++++++++++ test/move_base2_scenario_test.cpp | 115 ++++++ test/navigation_server_test.cpp | 302 ++++++++++++++++ test/plugins/test_local_planner.cpp | 69 ++-- test/walking_skeleton_test.cpp | 109 ++++++ 50 files changed, 3280 insertions(+), 119 deletions(-) create mode 120000 config/runtime/config create mode 120000 config/runtime/costmap_common_params.yaml create mode 120000 config/runtime/costmap_global_params.yaml create mode 120000 config/runtime/costmap_global_params_plugins_no_virtual_walls.yaml create mode 120000 config/runtime/costmap_local_params.yaml create mode 120000 config/runtime/costmap_local_params_plugins_no_virtual_walls.yaml create mode 120000 config/runtime/custom_global_params.yaml create mode 120000 config/runtime/dock_global_params.yaml create mode 120000 config/runtime/hybrid_local_planner_params.yaml create mode 120000 config/runtime/maker_sources.yaml create mode 120000 config/runtime/mission_adapters_params.yaml create mode 100644 config/runtime/move_base_common_params.yaml create mode 120000 config/runtime/mppi_local_planner_params.yaml create mode 120000 config/runtime/mprim create mode 120000 config/runtime/pnkx_local_planner_params.yaml create mode 120000 config/runtime/priest_local_planner_params.yaml create mode 120000 config/runtime/recovery_behaviors_params.yaml create mode 120000 config/runtime/sbpl_global_params.yaml create mode 120000 config/runtime/stanley_local_planner_params.yaml create mode 120000 config/runtime/two_points_global_params.yaml create mode 100644 include/move_base2/io/costmap_exporter.h create mode 100644 include/move_base2/navigation_runtime.h create mode 100644 launch/README.md create mode 100644 launch/move_base2_control.launch create mode 100644 launch/move_base2_sim.launch create mode 100644 src/io/costmap_exporter.cpp create mode 100644 src/navigation_runtime.cpp create mode 100644 test/mission_adapter_bridge_test.cpp create mode 100644 test/move_base2_scenario_driver.h create mode 100644 test/move_base2_scenario_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5bb2364..174cc8c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,6 +129,9 @@ else() # MissionAdapterBridge include thẳng mission_adapters: chỗ duy nhất trong gói này biết tới nó. mission_adapters + + # scenario_test chạy kịch bản khai báo qua khung của nav_test_harness. + nav_test_harness ) find_library(TF3_LIBRARY @@ -199,7 +202,9 @@ add_library(move_base2_core SHARED src/runners/planner_runner.cpp src/runners/controller_runner.cpp src/io/sensor_gateway.cpp + src/io/costmap_exporter.cpp src/bridges/mission_adapter_bridge.cpp + src/navigation_runtime.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. @@ -363,6 +368,10 @@ if(BUILDING_WITH_CATKIN) FILES_MATCHING PATTERN "*.h" ) + install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + else() install(TARGETS move_base2_core move_base2 move_base2_noop_action_handler @@ -437,6 +446,7 @@ if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN) planner_runner_test controller_runner_test mission_adapter_bridge_test + move_base2_scenario_test # tên có tiền tố gói: nav_test_harness đã có target scenario_test ) foreach(test_name ${MOVE_BASE2_TESTS}) @@ -471,6 +481,8 @@ if(BUILD_MOVE_BASE2_TESTS AND BUILDING_WITH_CATKIN) 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" + # Kịch bản là DỮ LIỆU nằm ở nav_test_harness; test không được đoán đường dẫn tới nó. + MOVE_BASE2_SCENARIO_DIR="${CMAKE_CURRENT_SOURCE_DIR}/../nav_test_harness/scenarios" ) endif() endforeach() diff --git a/config/runtime/config b/config/runtime/config new file mode 120000 index 0000000..3c88618 --- /dev/null +++ b/config/runtime/config @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/config \ No newline at end of file diff --git a/config/runtime/costmap_common_params.yaml b/config/runtime/costmap_common_params.yaml new file mode 120000 index 0000000..c229788 --- /dev/null +++ b/config/runtime/costmap_common_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/costmap_common_params.yaml \ No newline at end of file diff --git a/config/runtime/costmap_global_params.yaml b/config/runtime/costmap_global_params.yaml new file mode 120000 index 0000000..58c8409 --- /dev/null +++ b/config/runtime/costmap_global_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/costmap_global_params.yaml \ No newline at end of file diff --git a/config/runtime/costmap_global_params_plugins_no_virtual_walls.yaml b/config/runtime/costmap_global_params_plugins_no_virtual_walls.yaml new file mode 120000 index 0000000..e1508d3 --- /dev/null +++ b/config/runtime/costmap_global_params_plugins_no_virtual_walls.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/costmap_global_params_plugins_no_virtual_walls.yaml \ No newline at end of file diff --git a/config/runtime/costmap_local_params.yaml b/config/runtime/costmap_local_params.yaml new file mode 120000 index 0000000..706ad61 --- /dev/null +++ b/config/runtime/costmap_local_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/costmap_local_params.yaml \ No newline at end of file diff --git a/config/runtime/costmap_local_params_plugins_no_virtual_walls.yaml b/config/runtime/costmap_local_params_plugins_no_virtual_walls.yaml new file mode 120000 index 0000000..c8b7367 --- /dev/null +++ b/config/runtime/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/costmap_local_params_plugins_no_virtual_walls.yaml \ No newline at end of file diff --git a/config/runtime/custom_global_params.yaml b/config/runtime/custom_global_params.yaml new file mode 120000 index 0000000..f37d263 --- /dev/null +++ b/config/runtime/custom_global_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/custom_global_params.yaml \ No newline at end of file diff --git a/config/runtime/dock_global_params.yaml b/config/runtime/dock_global_params.yaml new file mode 120000 index 0000000..1c173ac --- /dev/null +++ b/config/runtime/dock_global_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/dock_global_params.yaml \ No newline at end of file diff --git a/config/runtime/hybrid_local_planner_params.yaml b/config/runtime/hybrid_local_planner_params.yaml new file mode 120000 index 0000000..3dae731 --- /dev/null +++ b/config/runtime/hybrid_local_planner_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/hybrid_local_planner_params.yaml \ No newline at end of file diff --git a/config/runtime/maker_sources.yaml b/config/runtime/maker_sources.yaml new file mode 120000 index 0000000..eac467e --- /dev/null +++ b/config/runtime/maker_sources.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/maker_sources.yaml \ No newline at end of file diff --git a/config/runtime/mission_adapters_params.yaml b/config/runtime/mission_adapters_params.yaml new file mode 120000 index 0000000..21bbd8b --- /dev/null +++ b/config/runtime/mission_adapters_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/mission_adapters_params.yaml \ No newline at end of file diff --git a/config/runtime/move_base_common_params.yaml b/config/runtime/move_base_common_params.yaml new file mode 100644 index 0000000..b278cb3 --- /dev/null +++ b/config/runtime/move_base_common_params.yaml @@ -0,0 +1,60 @@ +position_planner_name: PriestLocalPlanner #HybridLocalPlanner MPPILocalPlanner PriestLocalPlanner PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner #StanleyDockingLocalPlanner PNKXDockingLocalPlanner +go_straight_planner_name: PNKXGoStraightLocalPlanner +rotate_planner_name: PNKXRotateLocalPlanner +base_local_planner: LocalPlannerAdapter +base_global_planner: SBPLLatticePlanner + +PriestLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: SBPLLatticePlanner #CustomPlanner SBPLLatticePlanner + +PNKXDockingLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner + +PNKXGoStraightLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner + +PNKXRotateLocalPlanner: + base_local_planner: LocalPlannerAdapter + base_global_planner: TwoPointsPlanner + +### replanning +controller_frequency: 30.0 # run controller at 30.0 Hz +controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan +planner_frequency: 0.0 # don't continually replan (only when controller failed) +planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s... +max_planning_retries: 0 # ... or after 10 attempts (whichever happens first) +oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s +oscillation_distance: 0.5 +## recovery behaviors +# +# Recovery của move_base cũ đã dừng: bộ behavior gen-2 (tick-based) khai ở +# `recovery_behaviors_params.yaml` và do `recovery_core::RecoveryRegistry` nạp, không phải khoá +# `recovery_behaviors` ở đây. +# +# Danh sách gen-1 đã được gỡ hẳn thay vì để lại: các entry cũ trỏ tên alias `RotateRecovery` / +# `ClearCostmapRecovery` vào file .so gen-2, trong khi loader ở đây import theo chữ ký gen-1 +# (`robot_nav_core::RecoveryBehavior`). Boost.DLL không kiểm kiểu qua ranh giới .so, nên hai bên +# không bao giờ gặp nhau ở compile time và lỗi chỉ hiện ra lúc chạy. Giữ lại khoá cũng làm hai file +# config tranh nhau cùng một tên alias. +recovery_behavior_enabled: false +recovery_behaviors: [ + {name: aggressive_reset, type: ClearCostmapRecovery}, + {name: conservative_reset, type: ClearCostmapRecovery}, + ] + +conservative_reset: + reset_distance: 3.0 # clear obstacles farther away than 3.0 m + invert_area_to_clear: true + +aggressive_reset: + reset_distance: 3.0 + +ClearCostmapRecovery: + library_path: librobot_clear_costmap_recovery + +MoveBase: + library_path: libmove_base2 diff --git a/config/runtime/mppi_local_planner_params.yaml b/config/runtime/mppi_local_planner_params.yaml new file mode 120000 index 0000000..f36bc1b --- /dev/null +++ b/config/runtime/mppi_local_planner_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/mppi_local_planner_params.yaml \ No newline at end of file diff --git a/config/runtime/mprim b/config/runtime/mprim new file mode 120000 index 0000000..23c5629 --- /dev/null +++ b/config/runtime/mprim @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/mprim \ No newline at end of file diff --git a/config/runtime/pnkx_local_planner_params.yaml b/config/runtime/pnkx_local_planner_params.yaml new file mode 120000 index 0000000..924f5cb --- /dev/null +++ b/config/runtime/pnkx_local_planner_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/pnkx_local_planner_params.yaml \ No newline at end of file diff --git a/config/runtime/priest_local_planner_params.yaml b/config/runtime/priest_local_planner_params.yaml new file mode 120000 index 0000000..e419557 --- /dev/null +++ b/config/runtime/priest_local_planner_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/priest_local_planner_params.yaml \ No newline at end of file diff --git a/config/runtime/recovery_behaviors_params.yaml b/config/runtime/recovery_behaviors_params.yaml new file mode 120000 index 0000000..eba7a10 --- /dev/null +++ b/config/runtime/recovery_behaviors_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/recovery_behaviors_params.yaml \ No newline at end of file diff --git a/config/runtime/sbpl_global_params.yaml b/config/runtime/sbpl_global_params.yaml new file mode 120000 index 0000000..fcbc839 --- /dev/null +++ b/config/runtime/sbpl_global_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/sbpl_global_params.yaml \ No newline at end of file diff --git a/config/runtime/stanley_local_planner_params.yaml b/config/runtime/stanley_local_planner_params.yaml new file mode 120000 index 0000000..d7ccf8a --- /dev/null +++ b/config/runtime/stanley_local_planner_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/stanley_local_planner_params.yaml \ No newline at end of file diff --git a/config/runtime/two_points_global_params.yaml b/config/runtime/two_points_global_params.yaml new file mode 120000 index 0000000..0a64170 --- /dev/null +++ b/config/runtime/two_points_global_params.yaml @@ -0,0 +1 @@ +../../../../pnkx_nav_core/config/two_points_global_params.yaml \ No newline at end of file diff --git a/include/move_base2/control_loop.h b/include/move_base2/control_loop.h index b9b481b..81d4e65 100644 --- a/include/move_base2/control_loop.h +++ b/include/move_base2/control_loop.h @@ -147,6 +147,12 @@ public: */ bool step(); + /// @brief Plan đang được bám. Rỗng khi chưa có plan nào — bên gọi phải tự kiểm. + const std::vector& latestPlan() const + { + return latest_plan_; + } + /// @brief Lệnh vận tốc phát ra ở cycle gần nhất. const robot_geometry_msgs::Twist& lastCommand() const { @@ -172,6 +178,12 @@ public: return state_machine_.state(); } + /// @brief Đang có yêu cầu chạy dở (kể cả đang pause) — false ngay khi kết quả được báo. + bool hasActiveRequest() const + { + return has_active_request_; + } + /// @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; diff --git a/include/move_base2/core/state_machine.h b/include/move_base2/core/state_machine.h index e63d01a..8c9f9fa 100644 --- a/include/move_base2/core/state_machine.h +++ b/include/move_base2/core/state_machine.h @@ -297,6 +297,26 @@ private: StateMachineOutput& out); /// @brief Vào state terminal và bật cờ báo kết quả đúng một lần. + /** + * @brief Nhận yêu cầu đang chờ và đưa máy vào state chạy tương ứng. + * + * Tách ra vì có HAI đường vào: từ @c kIdle (bình thường) và từ mọi state đang chạy khi có yêu cầu + * mới chen ngang (preempt). + */ + void acceptPendingRequest(const StateMachineInput& in, StateMachineOutput& out); + + /** + * @brief Yêu cầu mới chen ngang yêu cầu đang chạy. + * + * @return true nếu đã preempt — bên gọi phải `break` ngay, mọi xử lý còn lại của state cũ không + * còn nghĩa. + * + * Goal mới phải thay goal cũ **ngay**, không xếp hàng: người vận hành bấm goal mới nghĩa là goal + * cũ không còn muốn nữa, và chờ tới khi robot đi hết chặng cũ là hành vi không ai mong đợi. Bản + * cũ preempt ngay qua action server; mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2). + */ + bool preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out); + void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now, const char* reason, StateMachineOutput& out); diff --git a/include/move_base2/io/costmap_exporter.h b/include/move_base2/io/costmap_exporter.h new file mode 100644 index 0000000..0046d07 --- /dev/null +++ b/include/move_base2/io/costmap_exporter.h @@ -0,0 +1,91 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — kết xuất costmap ra dạng OccupancyGrid cho host publish lên rviz. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_IO_COSTMAP_EXPORTER_H_ +#define MOVE_BASE2_IO_COSTMAP_EXPORTER_H_ + +#include +#include + +#include +#include + +namespace robot_costmap_2d +{ +class Costmap2DROBOT; +} + +namespace move_base2 +{ + +/** + * @class CostmapExporter + * @brief Chuyển lưới chi phí sang `OccupancyGrid` (bản đầy đủ) hoặc `OccupancyGridUpdate` (phần + * thay đổi), đúng giao thức mà host đang dùng để publish lên rviz. + * + * ## Vì sao LUÔN gửi lưới đầy đủ + * + * Giao thức `OccupancyGridUpdate` (chỉ gửi phần thay đổi) chỉ đúng khi có **đúng một** consumer. + * Host lại gọi `getGlobalData()` từ HAI ros::Timer — costmap (1 Hz) và plan (10 Hz) — nên timer plan + * tiêu thụ hầu hết các lần gọi rồi vứt phần costmap đi. Tới lượt timer costmap, vùng thay đổi đã bị + * reset, và lưới cache chỉ được dựng lại khi hình học đổi: local costmap là rolling window nên vẫn + * tươi, còn **global costmap đóng băng ở ảnh chụp đầu tiên vĩnh viễn**. Đã xảy ra. + * + * Bản cũ có cờ `always_send_full_costmap` cho đúng tình huống này. Ở đây chọn luôn gửi đầy đủ: + * 400×400 ô là 160 KB, ở 1–5 Hz là cái giá rẻ để đổi lấy việc không còn một lớp lỗi im lặng. + * + * ## Bảng quy đổi chi phí + * + * `costmap_2d` dùng thang 0–255 với vài giá trị đặc biệt; `OccupancyGrid` dùng −1 và 0–100. Bảng + * quy đổi giữ nguyên của bản cũ, kể cả hai giá trị riêng của workspace này (`PREFERRED_SPACE` → + * 120, `CRITICAL_SPACE` → 150) vốn nằm **ngoài** dải chuẩn 0–100 — rviz hiển thị chúng như ô lạ, + * nhưng đó là quy ước đang dùng và đổi nó là đổi thứ người vận hành đã quen nhìn. + * + * @note Thread-safe. Host gọi từ nhiều ros::Timer khác nhau (costmap và plan có timer riêng nhưng + * cùng gọi `getGlobalData`), nên trạng thái nội bộ phải được bảo vệ. + */ +class CostmapExporter +{ +public: + CostmapExporter(); + + /** + * @brief Gắn costmap nguồn. + * @param costmap **Non-owning**, được phép null (chưa dựng costmap thì kết xuất rỗng). + * @param global_frame Frame ghi vào header của lưới xuất ra. + */ + void attach(robot_costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame); + + /** + * @brief Điền lưới đầy đủ. + * + * @param[out] grid Lưới đầy đủ tại thời điểm gọi. + * @param[out] update Không dùng — giữ trong chữ ký vì contract host có trường này. + * @param[out] is_updated Luôn false: đây luôn là bản đầy đủ. Host dùng cờ này để chọn topic. + * + * Không làm gì khi chưa gắn costmap hoặc costmap còn kích thước 0 — @p grid giữ nguyên, để host + * không publish một lưới rỗng đè lên lưới đang hiển thị. + */ + void fill(robot_nav_msgs::OccupancyGrid& grid, robot_map_msgs::OccupancyGridUpdate& update, + bool& is_updated); + +private: + void prepareGridLocked(); + + mutable std::mutex mutex_; + robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; + std::string global_frame_ = "map"; + + robot_nav_msgs::OccupancyGrid grid_; + double saved_origin_x_ = 0.0; ///< [m] + double saved_origin_y_ = 0.0; ///< [m] +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_IO_COSTMAP_EXPORTER_H_ diff --git a/include/move_base2/navigation_runtime.h b/include/move_base2/navigation_runtime.h new file mode 100644 index 0000000..64ee417 --- /dev/null +++ b/include/move_base2/navigation_runtime.h @@ -0,0 +1,229 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — dựng toàn bộ thành phần runtime thật từ config và TF. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_NAVIGATION_RUNTIME_H_ +#define MOVE_BASE2_NAVIGATION_RUNTIME_H_ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tf3 +{ +class BufferCore; +} + +namespace move_base2 +{ + +/** + * @class CostmapPosePort + * @brief Cổng pose lấy từ costmap. + * + * `Costmap2DROBOT::getRobotPose` đã gói sẵn việc tra TF kèm `transform_tolerance` và trả false khi + * transform quá cũ — đúng ngữ nghĩa mà @ref PosePort cần, nên không có gì phải viết lại. + * + * @note Con trỏ costmap là **non-owning**. Lấy lại mỗi lần dùng chứ không cache `Costmap2D*` bên + * trong: con trỏ costmap có thể bị thay. + */ +class CostmapPosePort final : public PosePort +{ +public: + explicit CostmapPosePort(robot_costmap_2d::Costmap2DROBOT* costmap = nullptr) : costmap_(costmap) + { + } + + void setCostmap(robot_costmap_2d::Costmap2DROBOT* costmap) + { + costmap_ = costmap; + } + + bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override + { + if (costmap_ == nullptr) + { + return false; + } + return costmap_->getRobotPose(pose); + } + +private: + robot_costmap_2d::Costmap2DROBOT* costmap_; +}; + +/** + * @class NavigationRuntime + * @brief Sở hữu và dựng mọi thành phần thật của runtime: costmap, runner, bridge. + * + * Tách khỏi @ref NavigationServer để chỗ **lắp ráp** không lẫn với chỗ **hiện thực contract host**. + * `NavigationServer` vẫn nhận cổng qua `configureLoop`, nên nó kiểm được bằng cổng giả y như trước; + * lớp này chỉ là một cách dựng bộ cổng đó từ config thật. + * + * ## Thứ tự dựng là bắt buộc, không phải tuỳ ý + * + * `state_machine.recovery_behavior_count` **không đến từ YAML** — nó là số behavior + * `RecoveryRunner` nạp được **thật**. Đọc từ config 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. + * Trình tự bắt buộc, được @ref build tuân thủ: + * + * `MoveBase2Config::load()` → `RecoveryRunner::configure()` → gán `recovery_behavior_count` + * → `MoveBase2Config::validate()` + * + * ## Costmap + * + * Hai costmap được dựng ở đây và **chỉ `start()` sau khi mọi thứ khác đã sẵn sàng**: `start()` cho + * chạy thread cập nhật của costmap, và một thread cập nhật chạy trong khi planner chưa nạp xong là + * cửa sổ để mọi thứ chạm vào nhau ở trạng thái nửa vời. + */ +class NavigationRuntime +{ +public: + NavigationRuntime(); + ~NavigationRuntime(); + + NavigationRuntime(const NavigationRuntime&) = delete; + NavigationRuntime& operator=(const NavigationRuntime&) = delete; + + /** + * @brief Pha 1: đọc config và dựng hai costmap. **Chưa nạp plugin nào.** + * + * Tách khỏi @ref buildRunners là bắt buộc, không phải để cho gọn. `SBPLLatticePlanner::initialize` + * **chặn** cho tới khi costmap có kích thước khác 0 (`sbpl_lattice_planner.cpp:132`), mà kích + * thước đó chỉ được đặt khi static map tới `StaticLayer`. Nếu nạp planner ngay trong một pha thì + * planner chờ map, còn map thì chờ pha đó xong để được đẩy vào — khoá chết, và SBPL `exit(1)` sau + * 2 giây, giết cả tiến trình. + * + * Trình tự đúng, giống hệt bản cũ (`move_base.cpp:260` dựng costmap → `:287` đẩy map → `:306` nạp + * planner): dựng costmap → gắn vào SensorGateway và phát lại static map → rồi mới nạp plugin. + * + * @param tf Buffer TF do host cấp. Bắt buộc khác null; phải sống lâu hơn object này. + * @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false. + */ + bool buildCostmaps(const std::shared_ptr& tf, std::string& error); + + /** + * @brief Pha 2: nạp planner, controller, recovery, action rồi kiểm cấu hình. + * + * Chỉ gọi sau khi static map đã được đẩy vào costmap — xem @ref buildCostmaps. + * + * Thất bại ở bất kỳ bước nào cũng để runtime ở trạng thái **chưa dựng** (@ref built false), chứ + * không phải nửa dựng nửa không. + */ + bool buildRunners(std::string& error); + + /// @brief Hai costmap đã dựng xong chưa (pha 1). + bool costmapsReady() const + { + return global_costmap_ != nullptr && local_costmap_ != nullptr; + } + + bool built() const + { + return built_; + } + + /// @brief Cho costmap bắt đầu cập nhật. Gọi sau khi control loop đã được cấu hình xong. + void start(); + + /// @brief Dừng cập nhật costmap. + void stop(); + + /// @brief Bộ cổng để bơm vào @ref ControlLoop. Rỗng nếu chưa @ref build. + ControlLoopDeps deps(); + + const MoveBase2Config& config() const + { + return config_; + } + + robot_costmap_2d::Costmap2DROBOT* globalCostmap() + { + return global_costmap_.get(); + } + + robot_costmap_2d::Costmap2DROBOT* localCostmap() + { + return local_costmap_.get(); + } + + MissionAdapterBridge& mission() + { + return mission_; + } + + PlannerRunner& planner() + { + return planner_; + } + + ControllerRunner& controller() + { + return controller_; + } + + /// @brief Bộ kết xuất costmap cho host publish lên rviz. + CostmapExporter& globalExporter() + { + return global_exporter_; + } + + CostmapExporter& localExporter() + { + return local_exporter_; + } + +private: + MoveBase2Config config_; + bool built_ = false; + + std::shared_ptr tf_; + + // Costmap phải được khai TRƯỚC các runner: runner giữ con trỏ tới chúng, nên chúng phải bị huỷ + // SAU. Thứ tự khai báo thành viên chính là thứ tự huỷ ngược. + std::unique_ptr global_costmap_; + std::unique_ptr local_costmap_; + + SystemClock clock_; + + /** + * HAI nguồn pose, khác FRAME — không gộp được. + * + * `Costmap2DROBOT::getRobotPose` trả pose trong **global frame của chính costmap đó**: costmap + * global dùng `map`, costmap local dùng `odom`. Global planner lập plan trên lưới `map` nên start + * phải ở `map`; local planner và recovery làm việc trên lưới `odom` nên pose phải ở `odom`. + * + * Dùng nhầm nguồn không gây lỗi nào nhìn thấy được — chỉ là start của planner nằm ở gốc odom thay + * vì vị trí thật, và planner báo "failed to set start state". Đã xảy ra một lần. + */ + CostmapPosePort global_pose_; ///< frame `map` — planner, chống quẩn, pose báo về host. + CostmapPosePort local_pose_; ///< frame `odom` — controller và recovery. + CostmapExporter global_exporter_; + CostmapExporter local_exporter_; + + PlannerRunner planner_; + ControllerRunner controller_; + RecoveryRunner recovery_; + ActionRunner action_; + MissionAdapterBridge mission_; +}; + +} // namespace move_base2 + +#endif // MOVE_BASE2_NAVIGATION_RUNTIME_H_ diff --git a/include/move_base2/navigation_server.h b/include/move_base2/navigation_server.h index 3f851ac..f0cb2ec 100644 --- a/include/move_base2/navigation_server.h +++ b/include/move_base2/navigation_server.h @@ -9,9 +9,11 @@ #ifndef MOVE_BASE2_NAVIGATION_SERVER_H_ #define MOVE_BASE2_NAVIGATION_SERVER_H_ +#include #include #include #include +#include #include #include @@ -20,6 +22,7 @@ #include #include #include +#include namespace move_base2 { @@ -59,9 +62,36 @@ public: 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. + /** + * @brief Chạy một control cycle. @return false khi yêu cầu hiện tại vừa kết thúc. + * + * Ở runtime thật, hàm này do control thread nội bộ gọi (@ref startControlThread). Phơi ra public + * để test tự lái từng cycle mà không cần thread. + */ bool spinOnce(); + /** + * @brief Khởi động control thread — vòng lặp gọi @ref spinOnce ở @p frequency. + * + * Contract `BaseNavigation` **không có hàm spin nào**: host nạp plugin, gọi `initialize()`, rồi + * chỉ tương tác qua `moveTo`/`getTwist`/`getFeedback`. Bản `move_base` cũ tự lái mình bằng thread + * planner riêng cộng action server; runtime này vì thế cũng phải tự lái. Không có thread này thì + * goal được xếp vào chỗ chờ rồi nằm im vĩnh viễn — không cycle nào chạy, không state nào đổi, + * không lệnh nào phát, và **không log gì cả**. + * + * @param frequency [Hz] Nhịp control loop. <= 0 bị từ chối. + * @return false nếu chưa configure, thread đã chạy, hoặc nhịp không hợp lệ. + */ + bool startControlThread(double frequency); + + /// @brief Dừng và join control thread. An toàn khi gọi nhiều lần. + void stopControlThread(); + + bool controlThreadRunning() const + { + return control_thread_running_.load(); + } + const ControlLoop& loop() const { return loop_; @@ -115,6 +145,15 @@ public: void initialize(robot::TFListenerPtr tf) override; + /** + * @brief Dừng control thread và cập nhật costmap — quiesce trước khi host thả object. + * + * Cho host một điểm dừng chủ động TRONG trình tự tắt của nó, khi tiến trình còn sống đầy đủ; + * phần join các thread còn lại (planner, map update) nằm ở destructor. Sau lời gọi này object + * không phát lệnh nữa nhưng các getter (getFeedback, getTwist...) vẫn gọi được. Idempotent. + */ + void shutdown() override; + void setRobotFootprint(const std::vector& fprt) override; std::vector getRobotFootprint() override; @@ -204,11 +243,50 @@ private: */ void pushHostInputsToController(); + /** + * @brief Chuyển các yêu cầu pause/resume/cancel mà host đã đặt xuống lõi. + * + * Gọi từ @ref spinOnce, tức control thread. Host gọi `pause`/`resume`/`cancel` từ thread OPC-UA + * và VDA5050; `ControlLoop` không thread-safe nên chúng chỉ được ghi nhận, không chuyển tiếp ngay. + */ + void drainLifecycleRequests(); + + /** + * @brief Điền plan và footprint vào dữ liệu xuất cho host. + * + * @param exporter Nguồn lưới costmap; null thì chỉ điền plan và footprint (chế độ test, chưa có + * runtime thật). + */ + /** + * @brief Chụp plan toàn cục và quỹ đạo cục bộ vào bộ đệm. Chạy trên **control thread**. + * + * Lấy quỹ đạo cục bộ nghĩa là gọi vào plugin controller; plugin không thread-safe nên việc đó chỉ + * được làm ở đây, không phải trong getter mà host gọi từ ros::Timer. + */ + void cachePlans(); + + /** + * @brief Dựng dữ liệu hiển thị cho host, trả về **theo giá trị**. + * + * @warning Tuyệt đối không ghi vào `global_data_`/`local_data_`: host gọi hai getter này từ BỐN + * ros::Timer khác nhau, hai trong số đó dùng chung một thành viên. Ghi đồng thời đã gây + * `std::bad_alloc` rồi hỏng heap và giết tiến trình. + */ + robot::move_base_core::PlannerDataOutput buildPlannerData(CostmapExporter* exporter, + bool local_plan); + /// @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_; + + /// Thành phần runtime thật, dựng trong @ref initialize. Null khi cổng được bơm từ ngoài (test). + std::unique_ptr runtime_; + + /// Control thread — thread DUY NHẤT chạy control loop và phát cmd_vel. + std::thread control_thread_; + std::atomic control_thread_running_{ false }; robot::TFListenerPtr tf_; /// Bảo vệ dữ liệu sensor và footprint: host ghi từ thread của nó, control loop đọc. @@ -220,6 +298,21 @@ private: /// 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"; + /** + * Thời điểm cycle gần nhất còn yêu cầu đang chạy — mốc cho cửa ân hạn của @ref publishCommand. + * Zero = chưa từng điều khiển, stamp của twist đứng yên từ đầu (host không publish /cmd_vel, + * teleop dùng được ngay từ lúc boot). + */ + robot::Time last_command_active_time_; + + /// Frame của plan toàn cục và của quỹ đạo cục bộ — chỉ dùng khi chính plan không mang frame. + std::string global_frame_ = "map"; + std::string local_frame_ = "odom"; + + /// Bộ đệm plan do control thread ghi, host đọc. Bảo vệ bởi @c data_mutex_. + robot_nav_2d_msgs::Path2D cached_global_plan_; + robot_nav_2d_msgs::Path2D cached_local_plan_; + /** * Trần vận tốc host vừa đặt, chờ được đẩy xuống controller ở cycle kế tiếp. * @@ -233,6 +326,11 @@ private: bool has_pending_linear_backward_ = false; bool has_pending_angular_ = false; + /// Yêu cầu vòng đời host đã đặt, chờ chuyển xuống lõi ở cycle kế tiếp. + bool pause_requested_ = false; + bool resume_requested_ = false; + bool cancel_requested_ = false; + std::string last_reject_reason_; }; diff --git a/include/move_base2/ports/controller_port.h b/include/move_base2/ports/controller_port.h index 0feb436..0c7a823 100644 --- a/include/move_base2/ports/controller_port.h +++ b/include/move_base2/ports/controller_port.h @@ -15,6 +15,7 @@ #include #include #include +#include namespace move_base2 { @@ -61,6 +62,17 @@ public: /// @brief Đã tới đích theo sai số đã đặt hay chưa. virtual bool isGoalReached() = 0; + /** + * @brief Quỹ đạo CỤC BỘ mà controller vừa sinh ra. + * + * Khác plan toàn cục: đây là đoạn đường ngắn controller thật sự đang bám, và là thứ người vận + * hành nhìn trên rviz để biết robot "đang định đi thế nào" ngay lúc này. Bản cũ lấy nó bằng + * `tc_->getPlan(...)` sau mỗi lần tính lệnh thành công (`move_base.cpp:2947`). + * + * @param[out] plan Rỗng nếu controller chưa sinh quỹ đạo nào. + */ + virtual void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) = 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. * diff --git a/include/move_base2/runners/controller_runner.h b/include/move_base2/runners/controller_runner.h index 3000920..0320d27 100644 --- a/include/move_base2/runners/controller_runner.h +++ b/include/move_base2/runners/controller_runner.h @@ -11,14 +11,17 @@ #include #include +#include #include #include #include #include -#include +#include +#include #include +#include namespace robot_costmap_2d { @@ -34,11 +37,31 @@ namespace move_base2 /** * @class ControllerRunner - * @brief Nạp và chạy local planner thế hệ 1 (`robot_nav_core::BaseLocalPlanner`) qua Boost.DLL. + * @brief Nạp và chạy local planner **thế hệ 2** (`robot_nav_core2::LocalPlanner`) 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 + * Đây là chỗ duy nhất trong gói biết tới `robot_nav_core2::LocalPlanner`. Lõi quyết định chỉ thấy * @ref ControllerPort. * + * ## Vì sao gen-2 chứ không phải gen-1 + * + * Mọi local planner trong workspace này (`PriestLocalPlanner`, `HybridLocalPlanner`, …) implement + * `robot_nav_core2::LocalPlanner`; bản `move_base` cũ gọi tới chúng qua + * `robot_nav_core_adapter::LocalPlannerAdapter` — một `BaseLocalPlanner` bọc ngoài. Vì thế YAML có + * hai khoá riêng: `base_local_planner: LocalPlannerAdapter` (cái bọc) và + * `position_planner_name: PriestLocalPlanner` (planner thật). + * + * `move_base2` bỏ lớp bọc và gọi thẳng gen-2 — nhưng **phải nạp đúng kiểu gen-2**. Nạp một planner + * gen-2 qua `import_alias` sẽ "thành công": Boost.DLL là dlsym + + * reinterpret_cast, không kiểm kiểu nào qua ranh giới `.so`. Mọi lời gọi ảo sau đó trúng sai slot + * vtable — không lỗi biên dịch, không exception, chỉ segfault ở cycle đầu tiên. Đã xảy ra một lần. + * + * ## Ba việc mà lớp bọc gen-1 từng làm, nay làm ở đây + * + * Theo đúng `LocalPlannerAdapter`: + * 1. `setPlan` phải tách **goal** ra khỏi plan (pose cuối) và gọi `setGoalPose` trước `setPlan`; + * 2. `computeVelocityCommands` và `isGoalReached` của gen-2 cần **pose hiện tại** làm tham số; + * 3. giữ cờ "đang có goal" — gen-2 không tự biết, và tính lệnh khi chưa có goal là vô nghĩa. + * * ## 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 @@ -83,8 +106,8 @@ public: * @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, + bool configure(const robot::NodeHandle& nh, const std::shared_ptr& tf, + robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose, const std::string& initial_controller, std::string& error); bool configured() const @@ -101,6 +124,7 @@ public: bool setPlan(const std::vector& plan) override; bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override; bool isGoalReached() override; + void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) 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; @@ -116,24 +140,31 @@ private: /// @brief Một plugin đã nạp: factory phải sống cùng instance — vứt factory là để `.so` unload. struct Loaded { - std::function factory; - robot_nav_core::BaseLocalPlanner::Ptr instance; + std::function factory; + robot_nav_core2::LocalPlanner::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); + robot_nav_core2::LocalPlanner* 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); + /// @brief Áp lại trần vận tốc đã lưu lên controller vừa đổi sang. + void applyPendingLimits(robot_nav_core2::LocalPlanner* controller); + + /// @brief Pose hiện tại dạng 2D. @return false khi mất TF — bên gọi phải dừng, không đi mù. + bool currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const; robot::NodeHandle nh_; - tf3::BufferCore* tf_ = nullptr; + std::shared_ptr tf_; robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr; + const PosePort* pose_ = nullptr; ///< Non-owning. bool configured_ = false; std::map controllers_; std::string active_name_; - robot_nav_core::BaseLocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_. + robot_nav_core2::LocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_. + + /// Gen-2 không tự biết đang có goal hay không; tính lệnh khi chưa có goal là vô nghĩa. + bool has_active_goal_ = false; /** * Trần vận tốc và vận tốc đo được gần nhất. diff --git a/launch/README.md b/launch/README.md new file mode 100644 index 0000000..a0c2e24 --- /dev/null +++ b/launch/README.md @@ -0,0 +1,110 @@ +# Chạy move_base2 trên sim + +Bộ launch này thay cho chuỗi lệnh của `move_base` cũ. Không sửa gì ở đường chạy cũ — hai runtime +chạy song song được, chọn bằng lệnh `roslaunch` nào được gọi. + +## Đường chạy cũ (giữ nguyên, để đối chiếu) + +```bash +roslaunch amr_startup robot_maze_world.launch +roslaunch amr_startup rviz.launch +roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch +export PNKX_NAV_CORE_CONFIG_DIR=/home/duongtd/T800_ws/src/AMR_T800/pnkx_nav_core/config +roslaunch amr_startup amr_control.launch +``` + +## Đường chạy move_base2 + +```bash +cd /home/duongtd/T800_ws && source devel/setup.bash +roslaunch move_base2 move_base2_sim.launch +``` + +Một lệnh, không cần `export` gì. Muốn tách terminal như cũ thì chạy từng phần: + +```bash +# Terminal 1 — Gazebo + model T800 + world (dùng lại launch cũ, không đổi) +roslaunch amr_startup robot_maze_world.launch + +# Terminal 2 — rviz (dùng lại config cũ) +rviz -d $(rospack find amr_startup)/rviz/navigation.rviz + +# Terminal 3 — depth image -> point cloud (dùng lại launch cũ) +roslaunch robot_depth_image_proc depth_image_proc_gazebo.launch + +# Terminal 4 — nav stack trên move_base2. KHÔNG export gì: launch tự đặt config dir. +roslaunch move_base2 move_base2_control.launch +``` + +### Tham số + +| Arg | Mặc định | Ghi chú | +|---|---|---| +| `world` | `maze` | `maze` \| `warehouse` \| `sehc` \| `empty` | +| `robot_type` | `T800` | quyết định footprint; `T800` = `[[0.583,±0.48],[-0.583,±0.48]]` | +| `gui` | `true` | `false` = `gzserver` headless, chạy được qua ssh | +| `rviz` | `true` | | +| `depth_proc` | `true` | tắt khi không cần đường depth camera | + +```bash +roslaunch move_base2 move_base2_sim.launch world:=warehouse gui:=false +``` + +## Đổi qua lại giữa hai runtime bằng cách nào + +`amr_control` nạp plugin navigation bằng cách tra symbol **`MoveBase`** — tên này hard-code trong +host (`amr_control.cpp:244-246`), không đọc từ config. Thư viện nào được nạp thì do khoá +`MoveBase: library_path` trong `move_base_common_params.yaml` quyết định, và file đó được tìm qua +`PNKX_NAV_CORE_CONFIG_DIR`. + +Vì vậy chuyển đổi = trỏ `PNKX_NAV_CORE_CONFIG_DIR` sang cây khác: + +| Runtime | `PNKX_NAV_CORE_CONFIG_DIR` | `MoveBase: library_path` | +|---|---|---| +| cũ | `pnkx_nav_core/config` | `libmove_base` | +| mới | `move_base2/config/runtime` | `libmove_base2` | + +`move_base2/config/runtime/` là **symlink tới toàn bộ** `pnkx_nav_core/config/`, trừ đúng một file +thật: `move_base_common_params.yaml`. Chọn symlink thay vì copy là có lý do — `CLAUDE.md` cảnh báo +rõ về hai cây config trùng tên ("file khớp đầu tiên thắng", "sửa mãi không có tác dụng vì sửa nhầm +bản cũ"). Với symlink, sửa tham số ở cây gốc là cả hai đường chạy cùng thấy; chỉ đúng một dòng thật +sự khác nhau. + +`libmove_base2.so` export **cả hai** alias `MoveBase` và `MoveBase2`, nên host không phải sửa dòng +nào. + +## Xem gì khi chạy + +Log khởi động của `amr_node` phải có, theo thứ tự: + +``` +[move_base2] dùng schema mới ... | ... đọc theo schema gen-1 của move_base_common_params.yaml +[move_base2] PlannerRunner: global planner đang dùng là '...' +[move_base2] ControllerRunner: local planner đang dùng là '...' +[move_base2] NavigationRuntime dựng xong: +``` + +Trên rviz: `/global_costmap/costmap/plan` và `/local_costmap/costmap/footprint` có dữ liệu. + +**Chưa có:** lưới costmap trên rviz. `getGlobalData`/`getLocalData` của move_base2 hiện điền `plan` +và `footprint` nhưng chưa chuyển costmap sang `OccupancyGrid` — xem `@warning` ở +`NavigationServer::fillPlannerData`. Path và footprint hiện được, costmap thì không. + +## Nếu treo ở lúc khởi động + +`amr_control.cpp:252-258` **chờ vô hạn** `getFeedback()->is_ready` sau khi gọi `initialize()`: + +```cpp +do { r.sleep(); ros::spinOnce(); } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready); +``` + +`NavigationRuntime::build()` hỏng thì `is_ready` giữ nguyên `false` và vòng lặp này quay mãi. Triệu +chứng là `amr_node` im lặng không tiến thêm. Đọc log ngay phía trên nó — `initialize()` luôn in lý +do thất bại trước khi trả về: + +``` +[move_base2] initialize() thất bại: +``` + +Nguyên nhân hay gặp: sai `PNKX_NAV_CORE_CONFIG_DIR`, thiếu `library_path` cho planner trong YAML, +hoặc `.so` của planner không có trong `devel/lib`. diff --git a/launch/move_base2_control.launch b/launch/move_base2_control.launch new file mode 100644 index 0000000..29d9633 --- /dev/null +++ b/launch/move_base2_control.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]] + + + [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]] + + + [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + + + [[1.65,-0.4],[1.65,0.4],[-0.35,0.4],[-0.35,-0.4]] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/move_base2_sim.launch b/launch/move_base2_sim.launch new file mode 100644 index 0000000..c4ca56c --- /dev/null +++ b/launch/move_base2_sim.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 6867238..d894fa7 100644 --- a/package.xml +++ b/package.xml @@ -70,7 +70,8 @@ yaml-cpp - nav_test_harness + nav_test_harness + nav_test_harness recovery_core recovery_core diff --git a/src/config/move_base2_config.cpp b/src/config/move_base2_config.cpp index 327a8a0..2ea2c14 100644 --- a/src/config/move_base2_config.cpp +++ b/src/config/move_base2_config.cpp @@ -105,6 +105,54 @@ void describeBinding(std::ostringstream& out, const char* name, const ProfileBin << " m yaw=" << binding.default_yaw_tolerance << " rad\n"; } +/// Dịch patience gen-1 sang gen-2. Gen-1: mốc + patience luôn ở quá khứ khi patience <= 0, tức là +/// "fail -> recovery NGAY". Gen-2: <= 0 nghĩa là TẮT đồng hồ — ngược nghĩa hoàn toàn. Giữ hành vi +/// cũ bằng cách dịch thành đúng một chu kỳ điều khiển (gen-1 cũng chỉ phản ứng theo cycle). +double legacyPatience(double value, double control_period_s, const char* key) +{ + if (value > 0.0) + { + return value; + } + robot::log_warning( + "[move_base2] legacy %s = %.3f: gen-1 hiểu là 'fail -> recovery ngay', gen-2 hiểu là 'tắt " + "đồng hồ'. Dịch thành một chu kỳ điều khiển (%.4f s) để giữ hành vi cũ.", + key, value, control_period_s); + return control_period_s; +} + +/// Đọc binding của một profile theo schema gen-1: tên local planner ở khoá `_planner_name` +/// tại root, global planner ở section con mang TÊN planner đó (thiếu thì dùng global mặc định). +void readLegacyBinding(robot::NodeHandle& nh, const std::string& name_key, + const std::string& default_global, double xy_tolerance, + double yaw_tolerance, ProfileBinding& binding) +{ + binding.default_xy_tolerance = xy_tolerance; + binding.default_yaw_tolerance = yaw_tolerance; + binding.global_planner_name = default_global; + + // Default để RỖNG chứ không lấy default gen-1 ("mkt_algorithm/..."): các plugin đó không tồn tại + // trong workspace, và profile không khai coi như không dùng — validate sẽ chặn nếu cả bốn rỗng. + std::string local_name; + nh.param(name_key, local_name, std::string("")); + if (local_name.empty()) + { + robot::log_warning("[move_base2] legacy: thiếu '%s' — profile này bị tắt", name_key.c_str()); + return; + } + binding.local_planner_name = local_name; + + robot::NodeHandle planner_nh(nh, local_name); + if (planner_nh.hasParam("base_global_planner")) + { + planner_nh.param("base_global_planner", binding.global_planner_name, + binding.global_planner_name); + } + robot::log_info("[move_base2] legacy: %s='%s' -> local='%s' global='%s'", name_key.c_str(), + local_name.c_str(), binding.local_planner_name.c_str(), + binding.global_planner_name.c_str()); +} + } // namespace void MoveBase2Config::fromNodeHandle(robot::NodeHandle& nh) @@ -243,6 +291,140 @@ std::string MoveBase2Config::describe() const return out.str(); } + +// ================================================================================================ +// Đọc theo schema gen-1 +// ================================================================================================ + +void MoveBase2Config::fromLegacyNodeHandle(robot::NodeHandle& nh) +{ + // Default của gen-1 khác gen-2 ở hai chỗ; chế độ legacy giữ default gen-1 để không đổi hành vi + // của một hệ đang chạy chỉ vì đổi runtime. + robot_base_frame = "base_footprint"; + position.default_xy_tolerance = 0.2; // [m] + position.default_yaw_tolerance = 0.2; // [rad] + + readDouble(nh, "controller_frequency", controller_frequency); + readDouble(nh, "planner_frequency", planner_frequency); + 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); + readInt(nh, "max_planning_retries", state_machine.max_planning_retries); + readBool(nh, "recovery_behavior_enabled", state_machine.recovery_enabled); + + readString(nh, "global_frame", global_frame); + readString(nh, "robot_base_frame", robot_base_frame); + + // Sai số ở root là default chung cho cả bốn profile. + double xy = position.default_xy_tolerance; + double yaw = position.default_yaw_tolerance; + readDouble(nh, "xy_goal_tolerance", xy); + readDouble(nh, "yaw_goal_tolerance", yaw); + + std::string root_global_planner; + readString(nh, "base_global_planner", root_global_planner); + + if (nh.hasParam("base_local_planner")) + { + std::string adapter; + nh.param("base_local_planner", adapter, adapter); + // `LocalPlannerAdapter` là cầu nhúng planner gen-2 vào move_base gen-1. move_base2 gọi thẳng + // interface gen-2 qua ControllerPort nên không cần cầu đó — bỏ qua CÓ LOG, để không ai tưởng + // khoá này vẫn đang có hiệu lực. + robot::log_warning("[move_base2] schema gen-1: bỏ qua base_local_planner='%s' — move_base2 gọi " + "thẳng local planner, không qua adapter.", adapter.c_str()); + } + + struct LegacyProfile + { + const char* key; + ProfileBinding* binding; + }; + const LegacyProfile profiles[] = { + { "position_planner_name", &position }, + { "docking_planner_name", &docking }, + { "go_straight_planner_name", &go_straight }, + { "rotate_planner_name", &rotate }, + }; + + for (const LegacyProfile& profile : profiles) + { + profile.binding->default_xy_tolerance = xy; + profile.binding->default_yaw_tolerance = yaw; + + if (!nh.hasParam(profile.key)) + { + robot::log_warning("[move_base2] schema gen-1: thiếu '%s', profile này sẽ từ chối mọi yêu cầu", + profile.key); + continue; + } + + std::string local_planner; + nh.param(profile.key, local_planner, local_planner); + profile.binding->local_planner_name = local_planner; + + // Global planner riêng của profile nằm trong namespace mang tên chính planner đó; thiếu thì + // rơi về khoá ở root. Đúng cách bản cũ tra (`NodeHandle(private_nh_, position_planner_name_)`). + profile.binding->global_planner_name = root_global_planner; + if (!local_planner.empty()) + { + robot::NodeHandle planner_nh(nh, local_planner); + if (planner_nh.hasParam("base_global_planner")) + { + std::string profile_global; + planner_nh.param("base_global_planner", profile_global, profile_global); + profile.binding->global_planner_name = profile_global; + } + } + } + + // Dịch tường minh khác biệt NGỮ NGHĨA của patience. Gen-1: 0 nghĩa là "fail -> recovery NGAY" + // (mốc + 0 luôn ở quá khứ). Gen-2: 0 nghĩa là "tắt đồng hồ" — ngược hẳn. 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. + const double one_cycle = controller_frequency > 0.0 ? 1.0 / controller_frequency : 0.05; // [s] + if (state_machine.planner_patience <= 0.0) + { + robot::log_warning("[move_base2] schema gen-1: planner_patience <= 0 được dịch thành %.3f s " + "(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle); + state_machine.planner_patience = one_cycle; + } + if (state_machine.controller_patience <= 0.0) + { + robot::log_warning("[move_base2] schema gen-1: controller_patience <= 0 được dịch thành %.3f s " + "(một chu kỳ điều khiển), không phải 'tắt'.", one_cycle); + state_machine.controller_patience = one_cycle; + } +} + +MoveBase2Config MoveBase2Config::load(robot::NodeHandle& root_nh) +{ + MoveBase2Config config; + + // Nhận diện schema bằng một khoá bắt buộc phải có ở cả hai. KHÔNG trộn từng khoá giữa hai schema: + // 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" mà hai cây + // config trùng tên của workspace này đã gây ra một lần. + robot::NodeHandle modern_nh(root_nh, "move_base2"); + if (modern_nh.hasParam("controller_frequency")) + { + robot::log_info("[move_base2] dùng schema mới (namespace 'move_base2')."); + config.fromNodeHandle(modern_nh); + return config; + } + + if (root_nh.hasParam("controller_frequency") || root_nh.hasParam("base_global_planner")) + { + robot::log_warning("[move_base2] không thấy namespace 'move_base2'; đọc theo schema gen-1 của " + "move_base_common_params.yaml."); + config.fromLegacyNodeHandle(root_nh); + return config; + } + + robot::log_error("[move_base2] không tìm thấy cấu hình nào — chạy với toàn bộ giá trị mặc định. " + "Kiểm PNKX_NAV_CORE_CONFIG_DIR và sự tồn tại của file config."); + return config; +} + ControlLoopConfig MoveBase2Config::toControlLoopConfig() const { ControlLoopConfig config; diff --git a/src/control_loop.cpp b/src/control_loop.cpp index 8ff1f00..073626f 100644 --- a/src/control_loop.cpp +++ b/src/control_loop.cpp @@ -11,6 +11,8 @@ #include #include +#include + namespace move_base2 { namespace @@ -400,6 +402,13 @@ bool ControlLoop::step() action_feedback_ = ActionFeedback::kIdle; // --- 3. Thi hành output --------------------------------------------------------------------- + // Nhãn của yêu cầu SẮP bị thay. Preempt bật cả `report_outcome` lẫn `accept_request` trong cùng + // một cycle, mà phần báo kết quả nằm sau phần nhận yêu cầu — không giữ lại id cũ ở đây thì kết + // quả của chặng cũ sẽ được báo dưới id của chặng MỚI, và mission layer mất dấu cả hai. + const std::uint64_t outgoing_mission_id = + has_active_request_ ? active_request_.mission_sequence_id : 0; + const bool had_active_request = has_active_request_; + if (output.accept_request) { active_request_ = pending_request_; @@ -408,6 +417,21 @@ bool ControlLoop::step() latest_plan_.clear(); has_outcome_ = false; + // Log một lần tại sườn nhận goal — không nằm trên đường lặp của control loop. + if (active_request_.has_goal) + { + robot::log_info("[move_base2] Nhận goal (mission %llu): x=%.3f y=%.3f frame=%s.\n", + static_cast(active_request_.mission_sequence_id), + active_request_.goal.pose.position.x, active_request_.goal.pose.position.y, + active_request_.goal.header.frame_id.c_str()); + } + else + { + robot::log_info("[move_base2] Nhận yêu cầu chỉ-action (mission %llu), %zu action.\n", + static_cast(active_request_.mission_sequence_id), + active_request_.actions.size()); + } + // 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(); @@ -546,19 +570,49 @@ bool ControlLoop::step() has_outcome_ = true; ++outcome_report_count_; - if (deps_.mission != nullptr && has_active_request_ && - active_request_.mission_sequence_id != 0) + // Log kết cục ra terminal — bản cũ báo "Goal reached!" tại đây, host và người vận hành đều + // dựa vào dòng này để biết chặng kết thúc ra sao. Chỉ chạy đúng một lần mỗi chặng. + switch (output.outcome) { - deps_.mission->reportOutcome(active_request_.mission_sequence_id, output.outcome); + case NavigationOutcome::kSucceeded: + robot::log_success("[move_base2] Goal reached (mission %llu).\n", + static_cast(outgoing_mission_id)); + break; + case NavigationOutcome::kPreempted: + robot::log_info("[move_base2] Goal bị thay bởi goal mới (mission %llu: PREEMPTED).\n", + static_cast(outgoing_mission_id)); + break; + case NavigationOutcome::kCancelled: + robot::log_info("[move_base2] Goal bị huỷ (mission %llu: CANCELLED).\n", + static_cast(outgoing_mission_id)); + break; + case NavigationOutcome::kFailed: + robot::log_error("[move_base2] Navigation thất bại (mission %llu: ABORTED): %s\n", + static_cast(outgoing_mission_id), + last_reason_ != nullptr ? last_reason_ : ""); + break; } - has_active_request_ = false; + if (deps_.mission != nullptr && had_active_request && outgoing_mission_id != 0) + { + deps_.mission->reportOutcome(outgoing_mission_id, output.outcome); + } + + // Preempt vừa báo kết quả chặng cũ VỪA nhận chặng mới trong cùng cycle. Chỉ xoá yêu cầu đang + // chạy khi thực sự không có chặng mới nào thay thế. + if (!output.accept_request) + { + has_active_request_ = false; + latest_plan_.clear(); + planner_running_ = false; + } cancel_requested_ = false; - latest_plan_.clear(); - planner_running_ = false; } - return !output.report_outcome; + // false = "yêu cầu hiện tại vừa kết thúc, không còn gì để chạy". Preempt CŨNG báo kết quả (cho + // chặng cũ) nhưng ngay lập tức nhận chặng mới — lúc đó runtime vẫn đang chạy, nên không được trả + // false. Điều kiện đúng là "vào state terminal", không phải "có báo kết quả". + return !isTerminal(output.state); } const char* ControlLoop::lastOutcome() const diff --git a/src/io/costmap_exporter.cpp b/src/io/costmap_exporter.cpp new file mode 100644 index 0000000..86b921f --- /dev/null +++ b/src/io/costmap_exporter.cpp @@ -0,0 +1,148 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt CostmapExporter. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include + +#include +#include +#include +#include + +namespace move_base2 +{ +namespace +{ + +/** + * @brief Bảng quy đổi 0–255 của costmap sang −1..100 của OccupancyGrid. + * + * Dựng một lần, bất biến. Bản cũ dùng `new char[256]` gán vào biến static rồi không bao giờ giải + * phóng; ở đây là một mảng có tuổi thọ tĩnh, không cấp phát động. + */ +const std::array& costTranslationTable() +{ + static const std::array table = [] { + std::array t{}; + + // Dải thường: 10..252 của costmap co về 1..98 của OccupancyGrid. + for (int i = static_cast(robot_costmap_2d::FREE_SPACE) + 1; + i < static_cast(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); ++i) + { + t[static_cast(i)] = static_cast(1 + (97 * (i - 1)) / 251); + } + + // Các giá trị đặc biệt, đặt SAU dải thường để không bị nó ghi đè. + t[static_cast(robot_costmap_2d::PREFERRED_SPACE)] = 120; + for (int i = static_cast(robot_costmap_2d::PREFERRED_SPACE) + 1; + i <= static_cast(robot_costmap_2d::FREE_SPACE); ++i) + { + t[static_cast(i)] = 0; // không có vật cản + } + t[static_cast(robot_costmap_2d::CRITICAL_SPACE)] = 150; + t[static_cast(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)] = 99; + t[static_cast(robot_costmap_2d::LETHAL_OBSTACLE)] = 100; + t[static_cast(robot_costmap_2d::NO_INFORMATION)] = -1; // chưa biết + + return t; + }(); + return table; +} + +} // namespace + +CostmapExporter::CostmapExporter() = default; + +void CostmapExporter::attach(robot_costmap_2d::Costmap2DROBOT* costmap, + const std::string& global_frame) +{ + std::lock_guard lock(mutex_); + costmap_ = costmap; + if (!global_frame.empty()) + { + global_frame_ = global_frame; + } +} + +void CostmapExporter::prepareGridLocked() +{ + robot_costmap_2d::Costmap2D* costmap = costmap_->getCostmap(); + const double resolution = costmap->getResolution(); // [m/cell] + + grid_.header.frame_id = global_frame_; + grid_.header.stamp = robot::Time::now(); + grid_.info.resolution = resolution; + grid_.info.width = costmap->getSizeInCellsX(); + grid_.info.height = costmap->getSizeInCellsY(); + + // Gốc của OccupancyGrid là GÓC ô (0,0), còn mapToWorld trả TÂM ô — lệch nửa ô. Bỏ qua phép trừ + // này thì cả bản đồ trên rviz dịch đi 2.5 cm ở độ phân giải 0.05 m, đủ để mọi thứ trông "gần + // đúng" mà không ai chỉ ra được sai ở đâu. + double wx = 0.0; + double wy = 0.0; + costmap->mapToWorld(0, 0, wx, wy); + grid_.info.origin.position.x = wx - resolution / 2.0; // [m] + grid_.info.origin.position.y = wy - resolution / 2.0; // [m] + grid_.info.origin.position.z = 0.0; + grid_.info.origin.orientation.w = 1.0; + + saved_origin_x_ = costmap->getOriginX(); + saved_origin_y_ = costmap->getOriginY(); + + grid_.data.resize(static_cast(grid_.info.width) * grid_.info.height); + + const unsigned char* data = costmap->getCharMap(); + const std::array& table = costTranslationTable(); + for (std::size_t i = 0; i < grid_.data.size(); ++i) + { + grid_.data[i] = table[data[i]]; + } +} + +void CostmapExporter::fill(robot_nav_msgs::OccupancyGrid& grid, + robot_map_msgs::OccupancyGridUpdate& /*update*/, bool& is_updated) +{ + is_updated = false; + + std::lock_guard lock(mutex_); + if (costmap_ == nullptr) + { + return; + } + + robot_costmap_2d::Costmap2D* costmap = costmap_->getCostmap(); + if (costmap == nullptr || costmap->getSizeInCellsX() == 0 || costmap->getSizeInCellsY() == 0) + { + // Costmap chưa nhận được static map. Trả về mà không ghi gì: publish một lưới rỗng sẽ xoá trắng + // thứ đang hiển thị trên rviz. + return; + } + + // Khoá costmap trong lúc đọc: thread cập nhật của costmap đang ghi vào chính mảng này. + boost::unique_lock costmap_lock(*(costmap->getMutex())); + + // LUÔN dựng lại lưới đầy đủ. + // + // Đường gửi phần-thay-đổi chỉ đúng khi có ĐÚNG MỘT consumer, mà host thì gọi getGlobalData() từ + // HAI ros::Timer: costmap (1 Hz) và plan (10 Hz). Timer plan tiêu thụ hầu hết các lần gọi rồi vứt + // phần costmap đi, nên tới lượt timer costmap thì vùng thay đổi đã bị reset — và lưới cache chỉ + // được dựng lại khi HÌNH HỌC đổi. Local costmap là rolling window nên gốc toạ độ dịch liên tục và + // vẫn tươi; global costmap hình học cố định nên đóng băng ở ảnh chụp đầu tiên vĩnh viễn. + // + // Bản cũ có cờ `always_send_full_costmap` cho đúng tình huống này. Ở đây chọn luôn gửi đầy đủ: + // 400x400 ô là 160 KB, ở 1-5 Hz là không đáng kể so với một lớp lỗi im lặng. + prepareGridLocked(); + grid = grid_; + is_updated = false; + +} + +} // namespace move_base2 diff --git a/src/move_base2_plugin.cpp b/src/move_base2_plugin.cpp index 7a98034..e21dbc2 100644 --- a/src/move_base2_plugin.cpp +++ b/src/move_base2_plugin.cpp @@ -33,3 +33,16 @@ robot::move_base_core::BaseNavigation::Ptr createMoveBase2() } // namespace move_base2 BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase2) + +/** + * Alias thứ hai, cùng trỏ vào một factory. + * + * `amr_control` import alias **"MoveBase"** — tên đó hard-code trong host + * (`amr_control.cpp:244-246`), không đọc từ config. Không export nó thì đổi + * `MoveBase: library_path: libmove_base2` sẽ hỏng lúc chạy với lỗi không tìm thấy symbol, và + * `boost::dll` không kiểm kiểu qua ranh giới `.so` nên thông báo lỗi cũng không nói được vì sao. + * + * Có cả hai alias nghĩa là chuyển đổi giữa hai runtime chỉ tốn đúng một dòng YAML, không phải sửa + * host. `MoveBase2` giữ lại cho host nào muốn nạp tường minh bản mới. + */ +BOOST_DLL_ALIAS(move_base2::createMoveBase2, MoveBase) diff --git a/src/navigation_runtime.cpp b/src/navigation_runtime.cpp new file mode 100644 index 0000000..e36eaba --- /dev/null +++ b/src/navigation_runtime.cpp @@ -0,0 +1,202 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — cài đặt NavigationRuntime. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include + +#include +#include +#include + +namespace move_base2 +{ + +NavigationRuntime::NavigationRuntime() = default; + +NavigationRuntime::~NavigationRuntime() +{ + // Dừng thread cập nhật costmap trước khi bất kỳ thành viên nào bị huỷ: các runner giữ con trỏ tới + // costmap, và costmap giữ thread đang chạy qua chuỗi layer. + stop(); +} + +bool NavigationRuntime::buildCostmaps(const std::shared_ptr& tf, + std::string& error) +{ + if (built_ || costmapsReady()) + { + error = "NavigationRuntime::buildCostmaps() gọi lần thứ hai"; + return false; + } + + if (!tf) + { + error = "NavigationRuntime cần TF buffer khác null"; + return false; + } + + tf_ = tf; + + // --- 1. Config --------------------------------------------------------------------------------- + robot::NodeHandle root_nh("~"); + config_ = MoveBase2Config::load(root_nh); + + // --- 2. Costmap -------------------------------------------------------------------------------- + // + // Dựng nhưng CHƯA start: thread cập nhật chạy trong lúc planner chưa nạp xong là cửa sổ để mọi + // thứ chạm vào nhau ở trạng thái nửa vời. start() nằm ở hàm riêng, gọi sau khi lắp xong. + try + { + global_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_)); + local_costmap_.reset(new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_)); + } + catch (const std::exception& ex) + { + error = std::string("không dựng được costmap: ") + ex.what(); + global_costmap_.reset(); + local_costmap_.reset(); + return false; + } + + global_costmap_->pause(); + local_costmap_->pause(); + + // Hai nguồn pose, khác frame — xem doc của thành viên. Bản cũ cũng vậy: `makePlan` lấy start từ + // `planner_costmap_robot_` (map), còn `LocalPlannerAdapter` lấy pose từ costmap local (odom). + global_pose_.setCostmap(global_costmap_.get()); + local_pose_.setCostmap(local_costmap_.get()); + + // Gắn ngay: host có thể hỏi dữ liệu hiển thị bất cứ lúc nào sau initialize(), kể cả trước khi + // costmap có nội dung. Exporter tự trả về "chưa có gì" thay vì lưới rỗng. + global_exporter_.attach(global_costmap_.get(), config_.global_frame); + local_exporter_.attach(local_costmap_.get(), local_costmap_->getGlobalFrameID()); + + return true; +} + +bool NavigationRuntime::buildRunners(std::string& error) +{ + if (built_) + { + error = "NavigationRuntime::buildRunners() gọi lần thứ hai"; + return false; + } + if (!costmapsReady()) + { + error = "buildRunners() gọi trước buildCostmaps()"; + return false; + } + + robot::NodeHandle root_nh("~"); + + // --- 3. Planner và controller ------------------------------------------------------------------ + if (!planner_.configure(root_nh, global_costmap_.get(), config_.position.global_planner_name, + error)) + { + global_costmap_.reset(); + local_costmap_.reset(); + return false; + } + + if (!controller_.configure(root_nh, tf_, local_costmap_.get(), &local_pose_, + config_.position.local_planner_name, error)) + { + global_costmap_.reset(); + local_costmap_.reset(); + return false; + } + + // --- 4. Recovery ------------------------------------------------------------------------------- + RecoveryRunner::Deps recovery_deps; + recovery_deps.clock = &clock_; + recovery_deps.pose = &local_pose_; + recovery_deps.local_costmap = local_costmap_.get(); + recovery_deps.global_costmap = global_costmap_.get(); + recovery_.setDeps(recovery_deps); + recovery_.setNamespace(config_.recovery_namespace); + + if (!recovery_.configure(root_nh)) + { + // Không dừng lại: một behavior hỏng không nên xoá sạch các đường phục hồi còn lại. + // `behaviorCount()` bên dưới phản ánh số nạp được THẬT, và `validate()` sẽ chặn nếu con số đó + // bằng 0 trong khi recovery vẫn đang bật. + robot::log_warning("[move_base2] NavigationRuntime: có behavior recovery nạp hỏng; chạy tiếp " + "với %zu behavior còn lại.\n", recovery_.behaviorCount()); + } + + // Ràng buộc thứ tự khởi tạo — xem doc của lớp. Con số này KHÔNG đến từ YAML. + config_.state_machine.recovery_behavior_count = recovery_.behaviorCount(); + + // --- 5. Action --------------------------------------------------------------------------------- + action_.setClock(&clock_); + action_.setNamespace(config_.action_namespace); + + if (!action_.configure(root_nh)) + { + robot::log_warning("[move_base2] NavigationRuntime: có action handler nạp hỏng; chạy tiếp với " + "%zu handler còn lại.\n", action_.handlerCount()); + } + + // --- 6. Kiểm cấu hình sau cùng ----------------------------------------------------------------- + if (!config_.validate(error)) + { + global_costmap_.reset(); + local_costmap_.reset(); + return false; + } + + robot::log_info("[move_base2] NavigationRuntime dựng xong:\n%s", config_.describe().c_str()); + + built_ = true; + return true; +} + +void NavigationRuntime::start() +{ + if (!built_) + { + return; + } + global_costmap_->start(); + local_costmap_->start(); + mission_.start(); +} + +void NavigationRuntime::stop() +{ + mission_.stop(); + if (local_costmap_) + { + local_costmap_->stop(); + } + if (global_costmap_) + { + global_costmap_->stop(); + } +} + +ControlLoopDeps NavigationRuntime::deps() +{ + ControlLoopDeps deps; + if (!built_) + { + return deps; + } + + deps.clock = &clock_; + deps.pose = &global_pose_; // planner start + chống quẩn + pose báo về host: frame `map` + deps.planner = &planner_; + deps.controller = &controller_; + deps.recovery = &recovery_; + deps.mission = &mission_; + deps.action = &action_; + return deps; +} + +} // namespace move_base2 diff --git a/src/navigation_server.cpp b/src/navigation_server.cpp index 595ab38..91e7185 100644 --- a/src/navigation_server.cpp +++ b/src/navigation_server.cpp @@ -39,7 +39,64 @@ NavigationServer::NavigationServer() nav_feedback_->is_ready = false; } -NavigationServer::~NavigationServer() = default; +NavigationServer::~NavigationServer() +{ + // Dừng control thread TRƯỚC khi bất kỳ thành viên nào bị huỷ: nó đang chạm loop_, runtime_ và + // sensors_ ở mỗi cycle. Phần join thread còn lại (planner, map update) nằm ở destructor của + // runtime_ và các thành viên, chạy ngay sau đây theo thứ tự khai báo. + shutdown(); +} + +void NavigationServer::shutdown() +{ + stopControlThread(); + if (runtime_ != nullptr) + { + runtime_->stop(); + } +} + +bool NavigationServer::startControlThread(double frequency) +{ + if (!loop_.initialized()) + { + robot::log_error("[move_base2] startControlThread() trước khi control loop được cấu hình.\n"); + return false; + } + if (control_thread_running_.load()) + { + return false; + } + if (!(frequency > 0.0)) + { + robot::log_error("[move_base2] controller_frequency phải > 0 [Hz], nhận %.3f.\n", frequency); + return false; + } + + control_thread_running_.store(true); + control_thread_ = std::thread([this, frequency]() { + robot::Rate rate(frequency); + while (control_thread_running_.load()) + { + // Bỏ qua giá trị trả về: false chỉ nghĩa là yêu cầu hiện tại vừa kết thúc, không phải lý do + // dừng vòng lặp — thread phải sống để nhận goal kế tiếp. + spinOnce(); + rate.sleep(); + } + }); + + robot::log_info("[move_base2] control thread chạy ở %.2f Hz.\n", frequency); + return true; +} + +void NavigationServer::stopControlThread() +{ + control_thread_running_.store(false); + if (control_thread_.joinable()) + { + control_thread_.join(); + } +} // ================================================================================================ // Cấu hình lõi @@ -57,6 +114,7 @@ bool NavigationServer::configureLoop(const ControlLoopConfig& config, const Cont robot_base_frame_ = config.robot_base_frame; + nav_feedback_->is_ready = true; nav_feedback_->feed_back_str = "sẵn sàng"; refreshFeedback(); @@ -102,9 +160,18 @@ bool NavigationServer::spinOnce() // 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(); + drainLifecycleRequests(); + + // Mission layer cất chặng mới trong bridge; đây là chỗ duy nhất nó được đẩy vào lõi, trên đúng + // control thread. + if (runtime_ != nullptr) + { + runtime_->mission().pumpPendingRequest(); + } const bool running = loop_.step(); publishCommand(); + cachePlans(); refreshFeedback(); return running; } @@ -116,14 +183,39 @@ void NavigationServer::publishCommand() // — 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. + // chứ không phải giờ hệ thống lúc gọi: host loại lệnh quá hạn (cửa 0.05 s, amr_publiser.cpp:361), + // 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. + // + // Và khi KHÔNG có yêu cầu nào, dấu thời gian cũng phải đứng yên — đóng dấu mọi cycle là host + // phát 0 lên /cmd_vel ở 20 Hz vĩnh viễn, đè chết teleop/joystick (rqt_robot_steering 10 Hz thua + // 2:1 — robot chỉ nhích rồi đứng im). Bản cũ chỉ đóng dấu trong executeCycle và ở + // publishZeroVelocity(), tức chỉ khi đang điều khiển; đây là cùng ngữ nghĩa. Giữ thêm một cửa + // ân hạn sau khi yêu cầu kết thúc để lệnh dừng cuối cùng chắc chắn lọt qua cửa 0.05 s của host + // — kết thúc mà đóng băng ngay thì robot giữ nguyên vận tốc chót vì lệnh 0 không bao giờ được + // publish. + constexpr double kStopGraceSeconds = 0.5; // [s] ~10 cửa sổ 0.05s của host + const robot_geometry_msgs::Twist& command = loop_.lastCommand(); + const robot::Time cycle_time = loop_.lastCycleTime(); + + if (loop_.hasActiveRequest()) + { + last_command_active_time_ = cycle_time; + } + + const bool commanding = + loop_.hasActiveRequest() || + (!last_command_active_time_.isZero() && + (cycle_time - last_command_active_time_).toSec() < kStopGraceSeconds); std::lock_guard lock(data_mutex_); twist_.velocity = robot_nav_2d_utils::twist3Dto2D(command); - twist_.header.stamp = loop_.lastCycleTime(); twist_.header.frame_id = robot_base_frame_; + if (commanding) + { + twist_.header.stamp = cycle_time; + } + // Không commanding: giữ nguyên stamp cũ — host thấy lệnh quá hạn và nhường /cmd_vel cho nguồn khác. } robot::move_base_core::State NavigationServer::toHostState(NavigationState state) @@ -187,14 +279,99 @@ 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()) + 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"; + // Cổng đã được bơm vào từ ngoài (test, hoặc host tự lắp). Không dựng đè lên. + return; } + + runtime_.reset(new NavigationRuntime()); + + std::string error; + + // --- Pha 1: config + costmap. CHƯA nạp plugin nào. ------------------------------------------- + if (!runtime_->buildCostmaps(tf_, error)) + { + runtime_.reset(); + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "không dựng được costmap: " + error; + robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str()); + return; + } + + if (!configureSensors(runtime_->config().sensors, error)) + { + runtime_.reset(); + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "cấu hình cảm biến lỗi: " + error; + return; + } + + // --- Gắn costmap và PHÁT LẠI static map, TRƯỚC khi nạp planner ------------------------------- + // + // Thứ tự này bắt buộc. `SBPLLatticePlanner::initialize` chặn cho tới khi costmap có kích thước + // khác 0 (`sbpl_lattice_planner.cpp:132`), mà kích thước đó chỉ được đặt khi static map tới + // `StaticLayer`. Nạp planner trước khi map vào được costmap là khoá chết: planner chờ map, map + // chờ planner xong. SBPL `exit(1)` sau 2 giây và giết cả tiến trình. + // + // Bản cũ đi đúng thứ tự này: dựng costmap (`move_base.cpp:260`) -> đẩy map (`:287`) -> nạp + // planner (`:306`). + // Frame của hai lưới lấy TỪ chính costmap, không đoán: global thường là `map`, local là `odom`, + // nhưng cả hai đều do config quyết định. + global_frame_ = runtime_->globalCostmap()->getGlobalFrameID(); + local_frame_ = runtime_->localCostmap()->getGlobalFrameID(); + + attachCostmaps(runtime_->globalCostmap()->getLayeredCostmap(), + runtime_->localCostmap()->getLayeredCostmap()); + + // --- Pha 2: nạp planner, controller, recovery, action ---------------------------------------- + if (!runtime_->buildRunners(error)) + { + runtime_.reset(); + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "không nạp được runtime: " + error; + robot::log_error("[move_base2] initialize() thất bại: %s\n", error.c_str()); + return; + } + + if (!configureLoop(runtime_->config().toControlLoopConfig(), runtime_->deps(), error)) + { + runtime_.reset(); + robot::log_error("[move_base2] initialize() không cấu hình được control loop: %s\n", + error.c_str()); + return; + } + + // Mission layer đẩy chặng xuống qua callback này. Callback chạy trên CONTROL THREAD — bridge cất + // chặng lại và chỉ nhả ra ở pumpPendingRequest(), nên submit() không bị gọi từ thread mission. + runtime_->mission().setRequestCallback([this](const NavigationRequest& request) { + std::string reason; + if (!loop_.submit(request, reason)) + { + last_reject_reason_ = reason; + robot::log_error("[move_base2] từ chối chặng mission %llu: %s\n", + static_cast(request.mission_sequence_id), + reason.c_str()); + } + }); + runtime_->mission().setCancelCallback([this]() { cancel(); }); + + // start() sau cùng: cho thread cập nhật costmap chạy khi mọi thứ khác đã lắp xong. + runtime_->start(); + + // Rồi mới cho control loop chạy. Đây là thứ biến một tập thành phần đã lắp thành một runtime + // đang chạy — thiếu nó thì goal chỉ nằm trong chỗ chờ. + if (!startControlThread(runtime_->config().controller_frequency)) + { + runtime_.reset(); + nav_feedback_->is_ready = false; + nav_feedback_->feed_back_str = "không khởi động được control thread"; + return; + } + + nav_feedback_->is_ready = true; + nav_feedback_->feed_back_str = "sẵn sàng"; + refreshFeedback(); } // ================================================================================================ @@ -497,19 +674,60 @@ bool NavigationServer::rotateTo(const robot_geometry_msgs::PoseStamped& goal, // Điều khiển vòng đời // ================================================================================================ +// Ba hàm dưới đây được host gọi từ thread OPC-UA và VDA5050 (amr_control.cpp:159, 184), KHÔNG phải +// từ control thread. `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi hai cờ +// không nguyên tử — xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Vì vậy ở đây chỉ +// ghi nhận yêu cầu dưới `data_mutex_`; `spinOnce()` chuyển tiếp xuống lõi trên control thread. + void NavigationServer::pause() { - loop_.requestPause(); + std::lock_guard lock(data_mutex_); + pause_requested_ = true; + resume_requested_ = false; // Hai cờ đối nghịch, đặt cùng nhau dưới một lock. } void NavigationServer::resume() { - loop_.requestResume(); + std::lock_guard lock(data_mutex_); + resume_requested_ = true; + pause_requested_ = false; } void NavigationServer::cancel() { - loop_.requestCancel(); + std::lock_guard lock(data_mutex_); + cancel_requested_ = true; +} + +void NavigationServer::drainLifecycleRequests() +{ + bool pause = false; + bool resume = false; + bool cancel = false; + + { + std::lock_guard lock(data_mutex_); + pause = pause_requested_; + resume = resume_requested_; + cancel = cancel_requested_; + pause_requested_ = false; + resume_requested_ = false; + cancel_requested_ = false; + } + + // Huỷ trước: nó thắng mọi thứ khác. Tạm dừng rồi huỷ và huỷ rồi tạm dừng phải cho cùng kết quả. + if (cancel) + { + loop_.requestCancel(); + } + if (pause) + { + loop_.requestPause(); + } + if (resume) + { + loop_.requestResume(); + } } bool NavigationServer::setTwistLinear(const robot_geometry_msgs::Vector3& linear) @@ -640,14 +858,95 @@ robot::move_base_core::NavFeedback* NavigationServer::getFeedback() return nav_feedback_.get(); } +// `getGlobalData`/`getLocalData` KHÔNG phải getter thuần: bản cũ làm việc thật bên trong chúng +// (điền plan, chuyển costmap sang OccupancyGrid, cập nhật footprint) và host gọi từ bốn ros::Timer +// riêng để publish lên rviz — đây là toàn bộ đường xuất visualization. +// +// Đã truy hết consumer: **chỉ rviz**. OPC-UA và VDA5050 không đụng tới, không có gì trong control +// loop hay mission path đọc chúng. Vì vậy phần plan và footprint được điền ở đây (rẻ, không cần +// chuyển đổi gì), còn phần lưới costmap -> OccupancyGrid vẫn để trống — xem @warning ở header. + +void NavigationServer::cachePlans() +{ + // Chạy trên CONTROL THREAD. Đây là chỗ duy nhất chạm vào controller để lấy quỹ đạo cục bộ — + // `ControllerPort` không thread-safe và plugin (Priest) lại càng không. Gọi nó từ ros::Timer của + // host là chạm plugin song song với lúc control thread đang tính lệnh trên chính plugin đó. + // CHỈ hỏi khi controller thật sự đang bám plan. Hỏi mỗi cycle kể cả lúc rảnh nghĩa là gọi vào + // plugin 30 lần/giây để nhận về "chưa có gì" — PriestLocalPlanner log cảnh báo mỗi lần, và đó + // đúng là kiểu spam trong control loop mà quy ước của repo cấm. Không ở kControlling thì cũng + // không có quỹ đạo cục bộ nào để hiển thị. + robot_nav_2d_msgs::Path2D local; + ControllerPort* controller = loop_.controllerPort(); + if (controller != nullptr && loop_.state() == NavigationState::kControlling) + { + controller->getLocalPlan(local); + } + + robot_nav_2d_msgs::Path2D global = robot_nav_2d_utils::posesToPath2D(loop_.latestPlan()); + + // Đóng dấu bằng đồng hồ control loop: host bỏ qua plan cũ hơn 0.5 s và plan có dấu bằng 0 + // (amr_publiser.cpp:481-495). Loop treo thì dấu đứng yên và host ngừng publish — đúng ý. + const robot::Time stamp = loop_.lastCycleTime(); + global.header.stamp = stamp; + local.header.stamp = stamp; + if (global.header.frame_id.empty()) + { + global.header.frame_id = global_frame_; + } + if (local.header.frame_id.empty()) + { + local.header.frame_id = local_frame_; + } + + std::lock_guard lock(data_mutex_); + cached_global_plan_ = std::move(global); + cached_local_plan_ = std::move(local); +} + +robot::move_base_core::PlannerDataOutput NavigationServer::buildPlannerData(CostmapExporter* exporter, + bool local_plan) +{ + // Dựng vào một BẢN CỤC BỘ rồi trả về theo giá trị. Ghi thẳng vào `global_data_`/`local_data_` là + // hỏng thật: host gọi getGlobalData() và getLocalData() từ BỐN ros::Timer khác nhau, và hai timer + // dùng chung một thành viên sẽ ghi đè nhau giữa chừng — đã gây `std::bad_alloc` rồi + // `malloc(): invalid size` và giết cả tiến trình. + robot::move_base_core::PlannerDataOutput data; + + { + std::lock_guard lock(data_mutex_); + data.plan = local_plan ? cached_local_plan_ : cached_global_plan_; + + data.footprint.header.frame_id = robot_base_frame_; + data.footprint.polygon.points.reserve(footprint_.size()); + for (const auto& point : footprint_) + { + robot_geometry_msgs::Point32 p32; + p32.x = static_cast(point.x); + p32.y = static_cast(point.y); + p32.z = static_cast(point.z); + data.footprint.polygon.points.push_back(p32); + } + } + + if (exporter != nullptr) + { + // `CostmapExporter` tự khoá bên trong, nên gọi từ nhiều thread là an toàn. + exporter->fill(data.costmap, data.costmap_update, data.is_costmap_updated); + } + + return data; +} + robot::move_base_core::PlannerDataOutput NavigationServer::getGlobalData() { - return global_data_; + return buildPlannerData(runtime_ != nullptr ? &runtime_->globalExporter() : nullptr, + /*local_plan=*/false); } robot::move_base_core::PlannerDataOutput NavigationServer::getLocalData() { - return local_data_; + return buildPlannerData(runtime_ != nullptr ? &runtime_->localExporter() : nullptr, + /*local_plan=*/true); } } // namespace move_base2 diff --git a/src/runners/controller_runner.cpp b/src/runners/controller_runner.cpp index 73fa3bb..e68db30 100644 --- a/src/runners/controller_runner.cpp +++ b/src/runners/controller_runner.cpp @@ -17,6 +17,8 @@ #include #include +#include +#include namespace move_base2 { @@ -37,8 +39,9 @@ bool isFiniteTwist(const robot_geometry_msgs::Twist& twist) ControllerRunner::ControllerRunner() = default; ControllerRunner::~ControllerRunner() = default; -bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* tf, - robot_costmap_2d::Costmap2DROBOT* costmap, +bool ControllerRunner::configure(const robot::NodeHandle& nh, + const std::shared_ptr& tf, + robot_costmap_2d::Costmap2DROBOT* costmap, const PosePort* pose, const std::string& initial_controller, std::string& error) { if (configured_) @@ -53,9 +56,18 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* t return false; } + if (pose == nullptr) + { + // Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands/isGoalReached; không có nguồn pose + // thì không gọi được hàm nào trong hai hàm đó. + error = "ControllerRunner cần PosePort khác null"; + return false; + } + nh_ = nh; tf_ = tf; costmap_ = costmap; + pose_ = pose; configured_ = true; if (!initial_controller.empty() && !swapPlanner(initial_controller)) @@ -68,7 +80,7 @@ bool ControllerRunner::configure(const robot::NodeHandle& nh, tf3::BufferCore* t return true; } -robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& name) +robot_nav_core2::LocalPlanner* ControllerRunner::acquire(const std::string& name) { const auto cached = controllers_.find(name); if (cached != controllers_.end()) @@ -91,7 +103,7 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n try { - loaded.factory = boost::dll::import_alias( + loaded.factory = boost::dll::import_alias( library_path, name, boost::dll::load_mode::append_decorations); } catch (const boost::system::system_error& ex) @@ -126,9 +138,9 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n 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_); + // Chữ ký gen-2: (parent NodeHandle, tên, TF, costmap). Khác hẳn gen-1 — và vì Boost.DLL không + // kiểm kiểu, gọi nhầm chữ ký sẽ không lỗi biên dịch mà hỏng vtable lúc chạy. + loaded.instance->initialize(nh_, name, tf_, costmap_); } catch (const std::exception& ex) { @@ -141,7 +153,7 @@ robot_nav_core::BaseLocalPlanner* ControllerRunner::acquire(const std::string& n return inserted.first->second.instance.get(); } -void ControllerRunner::applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller) +void ControllerRunner::applyPendingLimits(robot_nav_core2::LocalPlanner* controller) { if (controller == nullptr) { @@ -191,7 +203,7 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name) 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); + robot_nav_core2::LocalPlanner* 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ề. @@ -200,6 +212,7 @@ bool ControllerRunner::swapPlanner(const std::string& planner_name) active_ = controller; active_name_ = planner_name; + has_active_goal_ = false; // Instance mới chưa biết goal nào. applyPendingLimits(active_); robot::log_info("[move_base2] ControllerRunner: local planner đang dùng là '%s'.\n", @@ -240,7 +253,23 @@ bool ControllerRunner::setPlan(const std::vectorsetPlan(plan); + // Gen-2 tách goal khỏi plan: `setGoalPose` phải gọi TRƯỚC `setPlan`, đúng như + // `LocalPlannerAdapter` làm. Goal là pose CUỐI của plan. + const robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(plan); + if (path.poses.empty()) + { + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: plan chuyển sang Path2D bị rỗng.\n"); + return false; + } + + // `Path2D::poses` đã là vector, nên pose cuối dùng thẳng làm goal. + const robot_nav_2d_msgs::Pose2DStamped goal_pose = path.poses.back(); + + active_->setGoalPose(goal_pose); + active_->setPlan(path); + has_active_goal_ = true; + return true; } catch (const std::exception& ex) { @@ -251,6 +280,21 @@ bool ControllerRunner::setPlan(const std::vectorgetRobotPose(stamped)) + { + return false; + } + pose = robot_nav_2d_utils::poseStampedToPose2D(stamped); + return true; +} + bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd) { cmd = robot_geometry_msgs::Twist(); @@ -263,23 +307,35 @@ bool ControllerRunner::computeVelocityCommands(robot_geometry_msgs::Twist& cmd) return false; } + if (!has_active_goal_) + { + // Chưa có plan nào được nạp. Không phải lỗi — chỉ là chưa tới lúc tính lệnh. + return false; + } + + robot_nav_2d_msgs::Pose2DStamped pose; + if (!currentPose(pose)) + { + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: mất pose, không tính lệnh.\n"); + return false; + } + robot_geometry_msgs::Twist result; - bool ok = false; try { - ok = active_->computeVelocityCommands(measured_velocity_, result); + // Gen-2 trả THẲNG lệnh (không có cờ thành công/thất bại) và ném exception khi không tính được — + // ngược với gen-1. Vì vậy nhánh "không có lệnh hợp lệ" ở đây là nhánh catch. + const robot_nav_2d_msgs::Twist2DStamped cmd_2d = + active_->computeVelocityCommands(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_)); + result = robot_nav_2d_utils::twist2Dto3D(cmd_2d.velocity); } 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) - { + "[move_base2] ControllerRunner: '%s' không sinh được lệnh: %s\n", + active_name_.c_str(), ex.what()); return false; } @@ -304,9 +360,26 @@ bool ControllerRunner::isGoalReached() return false; } + if (!has_active_goal_) + { + return false; + } + + robot_nav_2d_msgs::Pose2DStamped pose; + if (!currentPose(pose)) + { + return false; // Mất pose: "chưa tới đích" là phía an toàn. + } + try { - return active_->isGoalReached(); + const bool reached = + active_->isGoalReached(pose, robot_nav_2d_utils::twist3Dto2D(measured_velocity_)); + if (reached) + { + has_active_goal_ = false; + } + return reached; } catch (const std::exception& ex) { @@ -319,6 +392,30 @@ bool ControllerRunner::isGoalReached() } } +void ControllerRunner::getLocalPlan(robot_nav_2d_msgs::Path2D& plan) +{ + plan = robot_nav_2d_msgs::Path2D(); + + if (!configured_ || active_ == nullptr) + { + return; + } + + try + { + active_->getPlan(plan); + } + catch (const std::exception& ex) + { + // Không phải mọi planner đều hỗ trợ; gen-2 cho phép ném. Đây chỉ là dữ liệu hiển thị nên nuốt + // exception là đúng — nhưng vẫn log để không ai tưởng rviz đang hiện quỹ đạo thật. + robot::log_error_throttle(kHotPathLogThrottle, + "[move_base2] ControllerRunner: '%s' không trả được quỹ đạo cục bộ: " + "%s\n", active_name_.c_str(), ex.what()); + plan = robot_nav_2d_msgs::Path2D(); + } +} + void ControllerRunner::setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) { if (!isFiniteTwist(velocity)) diff --git a/src/state_machine.cpp b/src/state_machine.cpp index 2cd4178..41d7d3b 100644 --- a/src/state_machine.cpp +++ b/src/state_machine.cpp @@ -158,6 +158,68 @@ void StateMachine::finish(NavigationState terminal, NavigationOutcome outcome, enter(terminal, now, reason, out); } +void StateMachine::acceptPendingRequest(const StateMachineInput& in, StateMachineOutput& out) +{ + 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); + return; + } + out.start_action = true; + out.action_index = 0; + action_started_at_ = in.now; + enter(NavigationState::kExecutingActions, in.now, "yêu cầu chỉ có action", out); + return; + } + + 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); +} + +bool StateMachine::preemptIfRequested(const StateMachineInput& in, StateMachineOutput& out) +{ + if (!in.has_pending_request) + { + return false; + } + + // Dọn sạch yêu cầu cũ TRƯỚC khi nhận yêu cầu mới: planner đang chạy phải bị huỷ (kết quả của nó + // thuộc goal cũ), recovery và action đang chạy phải được bảo dừng an toàn. + out.stop_planner = true; + if (state_ == NavigationState::kRecovering) + { + out.cancel_recovery = true; + } + if (state_ == NavigationState::kExecutingActions) + { + out.cancel_action = true; + } + + // Báo kết quả cho chặng cũ ĐÚNG MỘT LẦN, ngay tại đây. Không báo thì mission layer chờ vĩnh viễn + // một chặng đã bị thay. + out.report_outcome = true; + out.outcome = NavigationOutcome::kPreempted; + + acceptPendingRequest(in, out); + return true; +} + StateMachineOutput StateMachine::update(const StateMachineInput& in) { StateMachineOutput out; @@ -193,36 +255,7 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) { 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); + acceptPendingRequest(in, out); } break; } @@ -244,6 +277,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) break; } + // Goal mới chen ngang: thay ngay, không xếp hàng. + if (preemptIfRequested(in, out)) + { + break; + } + if (in.planner == PlannerFeedback::kPlanReady) { out.apply_plan = true; @@ -299,6 +338,12 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) break; } + // Goal mới chen ngang: thay ngay, không xếp hàng. + if (preemptIfRequested(in, 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) { @@ -379,6 +424,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) // -------------------------------------------------------------------------------------- case NavigationState::kRecovering: { + // Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này — + // yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa. + if (preemptIfRequested(in, out)) + { + break; + } + if (in.cancel_requested) { out.cancel_recovery = true; @@ -417,6 +469,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) // -------------------------------------------------------------------------------------- case NavigationState::kExecutingActions: { + // Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này — + // yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa. + if (preemptIfRequested(in, out)) + { + break; + } + if (in.cancel_requested) { out.cancel_action = true; @@ -478,6 +537,13 @@ StateMachineOutput StateMachine::update(const StateMachineInput& in) // -------------------------------------------------------------------------------------- case NavigationState::kPaused: { + // Goal mới chen ngang: thay ngay, không xếp hàng. Đặt TRƯỚC mọi xử lý khác của state này — + // yêu cầu cũ đã bị thay thì mọi quyết định dựa trên nó đều vô nghĩa. + if (preemptIfRequested(in, out)) + { + break; + } + if (in.cancel_requested) { enter(NavigationState::kCancelling, in.now, "huỷ khi đang tạm dừng", out); diff --git a/test/config/move_base2_params.yaml b/test/config/move_base2_params.yaml index b9187be..847177f 100644 --- a/test/config/move_base2_params.yaml +++ b/test/config/move_base2_params.yaml @@ -169,3 +169,27 @@ TestControllerRefusesLimits: library_path: libmove_base2_test_local_planner # TestControllerMissing cố ý KHÔNG khai library_path. + +# --- Schema gen-1 (move_base_common_params.yaml) cho test đường legacy -------------------------- +legacy_move_base: + controller_frequency: 20.0 # [Hz] + planner_frequency: 0.0 # [Hz] + planner_patience: 0.0 # gen-1: "fail -> recovery NGAY", KHÔNG phải "tắt" + controller_patience: 15.0 # [s] + oscillation_timeout: 0.0 # [s] + oscillation_distance: 0.5 # [m] + max_planning_retries: 0 + recovery_behavior_enabled: true + + xy_goal_tolerance: 0.25 # [m] default chung cho cả bốn profile + yaw_goal_tolerance: 0.30 # [rad] + + base_global_planner: SBPLLatticePlanner + base_local_planner: LocalPlannerAdapter # phải bị BỎ QUA có log + + position_planner_name: HybridLocalPlanner + docking_planner_name: DockLocalPlanner + + # Global planner riêng của profile nằm trong namespace mang tên chính planner đó. + DockLocalPlanner: + base_global_planner: DockPlanner diff --git a/test/config_validation_test.cpp b/test/config_validation_test.cpp index b0aa437..9e2f65c 100644 --- a/test/config_validation_test.cpp +++ b/test/config_validation_test.cpp @@ -235,6 +235,105 @@ TEST(MoveBase2Config, DescribeMentionsEveryGroup) } // namespace +// ================================================================================================ +// Schema gen-1 (legacy) +// +// Cây config production hiện tại là schema gen-1. Không đọc được nó thì move_base2 không chạy được +// trên cấu hình thật, và bước 11 (chạy song song bản cũ/mới) không thực hiện được. +// ================================================================================================ + +TEST(MoveBase2ConfigLegacy, MapsPlannerNamesToProfiles) +{ + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + MoveBase2Config config; + config.fromLegacyNodeHandle(nh); + + EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner"); + EXPECT_EQ(config.docking.local_planner_name, "DockLocalPlanner"); + + // Profile không khai global planner riêng thì rơi về khoá ở root. + EXPECT_EQ(config.position.global_planner_name, "SBPLLatticePlanner"); + // Profile có khai riêng thì dùng bản riêng. + EXPECT_EQ(config.docking.global_planner_name, "DockPlanner"); +} + +TEST(MoveBase2ConfigLegacy, RootToleranceAppliesToEveryProfile) +{ + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + MoveBase2Config config; + config.fromLegacyNodeHandle(nh); + + EXPECT_DOUBLE_EQ(config.position.default_xy_tolerance, 0.25); + EXPECT_DOUBLE_EQ(config.docking.default_yaw_tolerance, 0.30); + EXPECT_DOUBLE_EQ(config.rotate.default_xy_tolerance, 0.25); +} + +TEST(MoveBase2ConfigLegacy, ZeroPatienceBecomesOneControlCycleNotDisabled) +{ + // Khác biệt NGỮ NGHĨA, không phải khác biệt giá trị. Gen-1: 0 = "fail -> recovery ngay". Gen-2: + // 0 = "tắt đồng hồ". Dịch nhầm là biến một cấu hình phản ứng-ngay thành cấu hình không-bao-giờ. + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + MoveBase2Config config; + config.fromLegacyNodeHandle(nh); + + EXPECT_GT(config.state_machine.planner_patience, 0.0); + EXPECT_NEAR(config.state_machine.planner_patience, 1.0 / 20.0, 1e-9); +} + +TEST(MoveBase2ConfigLegacy, KeepsGen1DefaultForRobotBaseFrame) +{ + // Gen-1 mặc định "base_footprint", gen-2 "base_link". Chế độ legacy giữ default gen-1 để không + // đổi hành vi của một hệ đang chạy chỉ vì đổi runtime. + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + MoveBase2Config config; + config.fromLegacyNodeHandle(nh); + + EXPECT_EQ(config.robot_base_frame, "base_footprint"); +} + +TEST(MoveBase2ConfigLegacy, LoadedLegacyConfigPassesValidation) +{ + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + MoveBase2Config config; + config.fromLegacyNodeHandle(nh); + config.state_machine.recovery_behavior_count = 2; + + std::string error; + EXPECT_TRUE(config.validate(error)) << error; +} + +TEST(MoveBase2ConfigLegacy, AutoDetectPrefersTheModernSchema) +{ + // Không trộn từng khoá giữa hai schema: 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" mà hai cây config trùng tên của workspace đã gây ra một lần. + robot::NodeHandle root; + const MoveBase2Config config = MoveBase2Config::load(root); + + EXPECT_EQ(config.robot_base_frame, "base_link") << "chọn nhầm schema gen-1 dù có namespace mới"; + EXPECT_TRUE(config.sensors.laser_sor_enabled) << "khoá chỉ có ở schema mới không được đọc"; +} + +TEST(MoveBase2ConfigLegacy, AutoDetectFallsBackToLegacyWhenNoModernNamespace) +{ + robot::NodeHandle root; + robot::NodeHandle nh(root, "legacy_move_base"); + + const MoveBase2Config config = MoveBase2Config::load(nh); + + EXPECT_EQ(config.robot_base_frame, "base_footprint"); + EXPECT_EQ(config.position.local_planner_name, "HybridLocalPlanner"); +} + int main(int argc, char** argv) { #ifdef MOVE_BASE2_TEST_CONFIG_DIR diff --git a/test/controller_runner_test.cpp b/test/controller_runner_test.cpp index d2ba9f1..2ace7ce 100644 --- a/test/controller_runner_test.cpp +++ b/test/controller_runner_test.cpp @@ -20,9 +20,25 @@ #include +#include "fake_ports.h" + namespace { using move_base2::ControllerRunner; +using move_base2::testing::FakePosePort; + +/// Pose cố định — gen-2 nhận pose làm tham số của computeVelocityCommands và isGoalReached. +FakePosePort& fixedPose() +{ + static FakePosePort pose; + static bool ready = false; + if (!ready) + { + pose.setPosition(0.0, 0.0); + ready = true; + } + return pose; +} /// [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; @@ -80,7 +96,7 @@ public: { robot::NodeHandle nh; std::string error; - ok_ = runner_.configure(nh, nullptr, dummyCostmap(), name, error); + ok_ = runner_.configure(nh, nullptr, dummyCostmap(), &fixedPose(), name, error); error_ = error; } @@ -113,18 +129,30 @@ TEST(ControllerRunner, RefusesNullCostmap) ControllerRunner runner; std::string error; - EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, "TestControllerOk", error)); + EXPECT_FALSE(runner.configure(nh, nullptr, nullptr, &fixedPose(), "TestControllerOk", error)); EXPECT_FALSE(error.empty()); EXPECT_FALSE(runner.configured()); } +TEST(ControllerRunner, RefusesNullPosePort) +{ + // Gen-2 nhận pose làm THAM SỐ của computeVelocityCommands và isGoalReached; không có nguồn pose + // thì không gọi được hàm nào trong hai hàm đó. + robot::NodeHandle nh; + ControllerRunner runner; + std::string error; + + EXPECT_FALSE(runner.configure(nh, nullptr, dummyCostmap(), nullptr, "TestControllerOk", error)); + 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.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "TestControllerMissing", error)); EXPECT_FALSE(runner.configured()) << "configure thất bại nhưng vẫn tự coi là đã cấu hình"; } @@ -209,7 +237,7 @@ TEST(ControllerRunner, LimitSetBeforeAControllerExistsIsAppliedOnceItIsLoaded) robot::NodeHandle nh; ControllerRunner runner; std::string error; - ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error; EXPECT_TRUE(runner.setTwistLinear(vec(0.08))); // [m/s], chưa có controller nào ASSERT_TRUE(runner.swapPlanner("TestControllerOk")); @@ -309,7 +337,7 @@ TEST(ControllerRunner, SetPlanWithoutAControllerFails) robot::NodeHandle nh; ControllerRunner runner; std::string error; - ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error; EXPECT_FALSE(runner.setPlan(makePlan())); } @@ -319,7 +347,7 @@ TEST(ControllerRunner, ComputeWithoutAControllerYieldsNoCommand) robot::NodeHandle nh; ControllerRunner runner; std::string error; - ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), "", error)) << error; + ASSERT_TRUE(runner.configure(nh, nullptr, dummyCostmap(), &fixedPose(), "", error)) << error; robot_geometry_msgs::Twist cmd; EXPECT_FALSE(runner.computeVelocityCommands(cmd)); diff --git a/test/fake_ports.h b/test/fake_ports.h index 8f68dbd..a44136d 100644 --- a/test/fake_ports.h +++ b/test/fake_ports.h @@ -375,6 +375,17 @@ public: measured_velocity_ = velocity; } + void getLocalPlan(robot_nav_2d_msgs::Path2D& plan) override + { + plan = local_plan_; + } + + /// @brief Đặt quỹ đạo cục bộ mà fake sẽ trả về. + void setLocalPlan(const robot_nav_2d_msgs::Path2D& plan) + { + local_plan_ = plan; + } + bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override { if (linear.x < 0.0) @@ -472,6 +483,7 @@ public: } private: + robot_nav_2d_msgs::Path2D local_plan_; robot_geometry_msgs::Twist measured_velocity_; double limit_forward_ = 0.0; ///< [m/s] double limit_backward_ = 0.0; ///< [m/s], âm diff --git a/test/mission_adapter_bridge_test.cpp b/test/mission_adapter_bridge_test.cpp new file mode 100644 index 0000000..6de85e2 --- /dev/null +++ b/test/mission_adapter_bridge_test.cpp @@ -0,0 +1,287 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — test MissionAdapterBridge. + * + * Hai thứ đáng khoá lại: phần chuyển đổi Mission -> NavigationRequest, và **biên thread** — mission + * layer đẩy chặng xuống từ thread của nó, nhưng chặng đó chỉ được chạm vào control loop trên control + * thread. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +using mission_adapters::Mission; +using mission_adapters::MissionManager; +using move_base2::MissionAdapterBridge; +using move_base2::NavigationOutcome; +using move_base2::NavigationRequest; + +std::shared_ptr makeMission(mission_adapters::MissionId id, double goal_x = 2.0, + std::size_t actions = 0, bool has_goal = true) +{ + auto mission = std::make_shared(); + mission->id = id; + mission->has_goal = has_goal; + mission->goal.header.frame_id = "map"; + mission->goal.pose.position.x = goal_x; // [m] + mission->goal.pose.orientation.w = 1.0; + + for (std::size_t i = 0; i < actions; ++i) + { + mission_adapters::Action action; + action.sequenceId = static_cast(i); + action.action.actionType = "act_" + std::to_string(i); + mission->actions.push_back(action); + } + return mission; +} + +/// @brief Bridge đã start, kèm chỗ nhận yêu cầu để test đọc lại. +class Fixture +{ +public: + Fixture() + { + bridge_.setRequestCallback([this](const NavigationRequest& request) { + received_.push_back(request); + }); + bridge_.setCancelCallback([this]() { ++cancel_calls_; }); + bridge_.start(); + } + + MissionAdapterBridge bridge_; + std::vector received_; + std::size_t cancel_calls_ = 0; +}; + +} // namespace + +// ================================================================================================ +// Chuyển đổi Mission -> NavigationRequest +// ================================================================================================ + +TEST(MissionAdapterBridgeConversion, CarriesIdGoalAndActions) +{ + const auto mission = makeMission(7, 3.5, 2); + const NavigationRequest request = MissionAdapterBridge::toRequest(*mission); + + EXPECT_EQ(request.mission_sequence_id, 7U); + EXPECT_TRUE(request.has_goal); + EXPECT_DOUBLE_EQ(request.goal.pose.position.x, 3.5); + ASSERT_EQ(request.actions.size(), 2U); + EXPECT_EQ(request.actions[0].actionType, "act_0"); + EXPECT_EQ(request.actions[1].actionType, "act_1"); +} + +TEST(MissionAdapterBridgeConversion, ActionOnlyMissionKeepsHasGoalFalse) +{ + // D8: mission chỉ-có-action. Navigation bỏ qua phần di chuyển và vào thẳng thực thi action. + const auto mission = makeMission(9, 0.0, 3, /*has_goal=*/false); + const NavigationRequest request = MissionAdapterBridge::toRequest(*mission); + + EXPECT_FALSE(request.has_goal); + EXPECT_EQ(request.actions.size(), 3U); +} + +TEST(MissionAdapterBridgeConversion, LeavesToleranceAtProfileDefault) +{ + // Quy ước của NavigationRequest: sai số <= 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. + const auto mission = makeMission(1); + const NavigationRequest request = MissionAdapterBridge::toRequest(*mission); + + EXPECT_FALSE(request.tolerance.hasXy()); + EXPECT_FALSE(request.tolerance.hasYaw()); +} + +// ================================================================================================ +// Biên thread: dispatch chỉ cất lại, control thread mới đẩy xuống +// ================================================================================================ + +TEST(MissionAdapterBridge, DispatchDoesNotReachNavigationUntilPumped) +{ + // `MissionExecutor` gọi dispatch từ thread của nó, còn ControlLoop không thread-safe. Đẩy thẳng + // xuống ở đây nghĩa là hai thread cùng ghi pending_request_ của control loop. + Fixture fixture; + + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(3))); + EXPECT_TRUE(fixture.received_.empty()) << "dispatch đi thẳng xuống navigation, bỏ qua biên thread"; + + EXPECT_TRUE(fixture.bridge_.pumpPendingRequest()); + ASSERT_EQ(fixture.received_.size(), 1U); + EXPECT_EQ(fixture.received_[0].mission_sequence_id, 3U); +} + +TEST(MissionAdapterBridge, PumpingWithNothingPendingIsANoOp) +{ + Fixture fixture; + + EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()); + EXPECT_TRUE(fixture.received_.empty()); +} + +TEST(MissionAdapterBridge, EachMissionIsPushedDownExactlyOnce) +{ + Fixture fixture; + + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(4))); + EXPECT_TRUE(fixture.bridge_.pumpPendingRequest()); + EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()) << "cùng một mission bị đẩy xuống hai lần"; + EXPECT_EQ(fixture.received_.size(), 1U); +} + +TEST(MissionAdapterBridge, DispatchBeforeStartIsRefused) +{ + // 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. + MissionAdapterBridge bridge; + EXPECT_FALSE(bridge.dispatch(makeMission(1))); +} + +TEST(MissionAdapterBridge, DispatchAfterStopIsRefused) +{ + Fixture fixture; + fixture.bridge_.stop(); + + EXPECT_FALSE(fixture.bridge_.dispatch(makeMission(1))); + EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()); +} + +TEST(MissionAdapterBridge, StopDropsAMissionThatWasStillWaiting) +{ + // Giữ lại để nó chạy sau một lần start() nào đó về sau là hành vi không ai mong đợi. + Fixture fixture; + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(5))); + + fixture.bridge_.stop(); + fixture.bridge_.start(); + + EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()); + EXPECT_TRUE(fixture.received_.empty()); +} + +TEST(MissionAdapterBridge, NullMissionIsRefused) +{ + Fixture fixture; + EXPECT_FALSE(fixture.bridge_.dispatch(nullptr)); +} + +TEST(MissionAdapterBridge, OverwritingAWaitingMissionIsCounted) +{ + // Không nên xảy ra — MissionManager chỉ giao chặng mới sau khi chặng cũ kết thúc. Nhưng im lặng ở + // đây nghĩa là một chặng biến mất trong khi fleet master vẫn chờ nó. + Fixture fixture; + + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(1))); + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2))); + + EXPECT_EQ(fixture.bridge_.droppedRequests(), 1U); + + ASSERT_TRUE(fixture.bridge_.pumpPendingRequest()); + ASSERT_EQ(fixture.received_.size(), 1U); + EXPECT_EQ(fixture.received_[0].mission_sequence_id, 2U) << "mission cũ thắng mission mới"; +} + +// ================================================================================================ +// Huỷ +// ================================================================================================ + +TEST(MissionAdapterBridge, CancelReachesNavigationThroughTheCallback) +{ + Fixture fixture; + fixture.bridge_.cancelActive(1); + + EXPECT_EQ(fixture.cancel_calls_, 1U); +} + +TEST(MissionAdapterBridge, CancelDropsAMissionThatHadNotBeenPushedDownYet) +{ + // Đẩ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. + Fixture fixture; + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(6))); + + fixture.bridge_.cancelActive(6); + + EXPECT_FALSE(fixture.bridge_.pumpPendingRequest()); + EXPECT_TRUE(fixture.received_.empty()); +} + +// ================================================================================================ +// Báo kết quả về mission layer +// ================================================================================================ + +TEST(MissionAdapterBridge, ReportingWithoutAManagerIsSafe) +{ + // Chạy không có mission layer là cấu hình hợp lệ: goal có thể đến thẳng từ contract host. + Fixture fixture; + EXPECT_NO_THROW(fixture.bridge_.reportOutcome(1, NavigationOutcome::kSucceeded)); +} + +TEST(MissionAdapterBridge, DirectGoalWithoutMissionIdIsNotReported) +{ + MissionManager manager; + Fixture fixture; + fixture.bridge_.attach(&manager); + + fixture.bridge_.reportOutcome(0, NavigationOutcome::kSucceeded); + + EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U) << "goal trực tiếp bị đem báo lên mission layer"; +} + +TEST(MissionAdapterBridge, SuccessReachesTheManagerAsNavigationDone) +{ + MissionManager manager; + Fixture fixture; + fixture.bridge_.attach(&manager); + + manager.submit({ makeMission(0) }); + const auto running = manager.nextMission(); + ASSERT_TRUE(running) << "manager không giao mission nào để chạy"; + + fixture.bridge_.reportOutcome(running->id, NavigationOutcome::kSucceeded); + + EXPECT_EQ(fixture.bridge_.staleOutcomes(), 0U); + EXPECT_EQ(manager.currentMissionId(), mission_adapters::kInvalidMissionId) + << "mission vẫn còn đang chạy sau khi đã báo hoàn tất"; +} + +TEST(MissionAdapterBridge, OutcomeForAMissionThatIsNoLongerRunningIsCounted) +{ + // Outcome tới trễ sau khi mission đã bị thay. Manager từ chối nó — đúng ý, nhưng phải đếm được, + // vì đếm tăng bất thường nghĩa là bất biến "báo đúng một lần" đang bị vi phạm ở đâu đó. + MissionManager manager; + Fixture fixture; + fixture.bridge_.attach(&manager); + + fixture.bridge_.reportOutcome(9999, NavigationOutcome::kSucceeded); + + EXPECT_EQ(fixture.bridge_.staleOutcomes(), 1U); +} + +TEST(MissionAdapterBridge, HasActiveMissionCountsAMissionStillWaitingToBePushedDown) +{ + Fixture fixture; + EXPECT_FALSE(fixture.bridge_.hasActiveMission()); + + ASSERT_TRUE(fixture.bridge_.dispatch(makeMission(2))); + EXPECT_TRUE(fixture.bridge_.hasActiveMission()); +} + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/move_base2_scenario_driver.h b/test/move_base2_scenario_driver.h new file mode 100644 index 0000000..f733952 --- /dev/null +++ b/test/move_base2_scenario_driver.h @@ -0,0 +1,294 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — hiện thực ScenarioDriver để chạy kịch bản khai báo của nav_test_harness. + * + * Đặt trong test/ của move_base2 chứ không trong harness: harness không được phụ thuộc ngược vào + * package tiêu thụ nào. Harness biết cách CHẠY và cách KIỂM; "chạy cái gì" do bên tiêu thụ hiện + * thực — đây chính là chỗ đó. + * + * Author: DuongTD + *********************************************************************/ +#ifndef MOVE_BASE2_TEST_SCENARIO_DRIVER_H_ +#define MOVE_BASE2_TEST_SCENARIO_DRIVER_H_ + +#include +#include +#include +#include + +#include +#include + +#include + +#include "fake_ports.h" + +namespace move_base2 +{ +namespace testing +{ + +/** + * @class MoveBase2ScenarioDriver + * @brief Chạy một kịch bản qua @ref ControlLoop với toàn bộ cổng giả. + * + * Kịch bản mô tả **hành vi của các cổng**, không mô tả thế giới vật lý: `planner_script` là chuỗi + * kết quả `makePlan`, `controller_script` là chuỗi kết quả `computeVelocityCommands`. Nhờ vậy các + * bất biến an toàn (mất TF -> dừng, NaN -> chặn, vượt trần -> clamp) kiểm được **xác định**, không + * phụ thuộc vào việc dựng đúng một tình huống hình học. + * + * @note `obstacles` trong kịch bản **chưa được dùng** ở driver này: cổng recovery là fake theo kịch + * bản nên không tra costmap. Kịch bản cần va chạm thật (`obstacle_behind_during_backup`) phải + * chạy qua một driver khác nạp `recovery_core` thật kèm `FakeCollisionChecker` — thuộc phần + * sau của Phase 5. Driver này **báo lỗi setup** nếu kịch bản khai obstacle, thay vì lặng lẽ + * chạy một ca test không kiểm đúng thứ nó nói là đang kiểm. + */ +class MoveBase2ScenarioDriver final : public nav_test_harness::ScenarioDriver +{ +public: + bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override + { + scenario_ = scenario; + + if (!scenario.obstacles.empty()) + { + error = "driver này không mô phỏng vật cản (cổng recovery là fake theo kịch bản); " + "dùng driver có recovery_core thật cho kịch bản va chạm"; + return false; + } + + std::vector planner_script; + for (const std::string& item : scenario.planner_script) + { + if (item == "ok") + { + planner_script.push_back(PlannerScript::kOk); + } + else if (item == "fail") + { + planner_script.push_back(PlannerScript::kFail); + } + else if (item == "empty") + { + planner_script.push_back(PlannerScript::kEmpty); + } + else + { + error = "planner_script không hiểu: '" + item + "'"; + return false; + } + } + + std::vector controller_script; + for (const std::string& item : scenario.controller_script) + { + if (item == "ok") + { + controller_script.push_back(ControllerScript::kOk); + } + else if (item == "fail") + { + controller_script.push_back(ControllerScript::kFail); + } + else if (item == "goal_reached") + { + controller_script.push_back(ControllerScript::kGoalReached); + } + else if (item == "nan") + { + controller_script.push_back(ControllerScript::kNaN); + } + else if (item == "too_fast") + { + controller_script.push_back(ControllerScript::kTooFast); + } + else + { + error = "controller_script không hiểu: '" + item + "'"; + return false; + } + } + + std::vector recovery_script; + for (const std::string& item : scenario.recovery_script) + { + if (item == "running") + { + recovery_script.push_back(RecoveryScript::kRunning); + } + else if (item == "succeeded") + { + recovery_script.push_back(RecoveryScript::kSucceeded); + } + else if (item == "failed") + { + recovery_script.push_back(RecoveryScript::kFailed); + } + else + { + error = "recovery_script không hiểu: '" + item + "'"; + return false; + } + } + + for (const nav_test_harness::ScenarioEvent& event : scenario.events) + { + if (event.action != "cancel" && event.action != "pause" && event.action != "resume" && + event.action != "lose_pose" && event.action != "restore_pose") + { + error = "events: action không hiểu: '" + event.action + "'"; + return false; + } + } + + planner_.setScript(planner_script); + controller_.setScript(controller_script); + recovery_.setScript(recovery_script); + + pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y); + + ControlLoopConfig config; + config.nominal_control_period = scenario.control_period; // [s] + config.state_machine.planner_patience = 0.5; // [s] + config.state_machine.controller_patience = 0.5; // [s] + config.state_machine.oscillation_timeout = 0.0; // tắt trừ khi kịch bản cần + 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; + + // Trần vận tốc lấy từ kỳ vọng của kịch bản: `expect_max_speed` vừa là điều được kiểm, vừa là + // trần mà arbiter phải áp — nếu hai con số đó lệch nhau thì ca test không kiểm đúng thứ nó nói. + config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5; + config.velocity.min_vel_x = -config.velocity.max_vel_x; + config.velocity.max_vel_theta = + scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0; + config.velocity.max_accel_x = 100.0; // [m/s^2] lớn: kịch bản kiểm state và trần, không kiểm ramp + config.velocity.max_accel_theta = 100.0; // [rad/s^2] + + config.position.global_planner_name = "ScenarioGlobalPlanner"; + config.position.local_planner_name = "ScenarioLocalPlanner"; + config.docking = config.position; + config.go_straight = config.position; + config.rotate = config.position; + + deps_.clock = &clock_; + deps_.pose = &pose_; + deps_.planner = &planner_; + deps_.controller = &controller_; + deps_.recovery = &recovery_; + deps_.mission = &mission_; + deps_.action = &action_; + + if (!loop_.configure(config, deps_, error)) + { + return false; + } + + NavigationRequest request; + request.profile = MotionProfile::kPosition; + request.goal.header.frame_id = "map"; + request.goal.pose.position.x = scenario.goal.x; // [m] + request.goal.pose.position.y = scenario.goal.y; // [m] + request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5); + request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5); + + if (!loop_.submit(request, error)) + { + return false; + } + + cycle_ = 0; + started_ = false; + return true; + } + + bool step(nav_test_harness::ScenarioStep& step) override + { + if (!started_) + { + // Ảnh chụp TRƯỚC khi cycle nào chạy. Không phải chi tiết kỹ thuật: nó là bằng chứng rằng lõi + // chưa phát lệnh nào trước khi nhận yêu cầu — và không có nó thì `expect_no_cmd_in_states` + // không kiểm được state ban đầu, vì state đó không bao giờ xuất hiện trong trace. + started_ = true; + step.cycle = 0; + step.state = toString(loop_.state()); + step.linear_x = 0.0; + step.angular_z = 0.0; + return true; + } + + applyEventsFor(cycle_); + + const bool running = loop_.step(); + + step.cycle = cycle_; + step.state = toString(loop_.state()); + step.linear_x = loop_.lastCommand().linear.x; // [m/s] + step.angular_z = loop_.lastCommand().angular.z; // [rad/s] + + clock_.advance(scenario_.control_period); + ++cycle_; + return running; + } + + std::string outcome() const override + { + const char* text = loop_.lastOutcome(); + return text != nullptr ? std::string(text) : std::string(); + } + +private: + void applyEventsFor(std::size_t cycle) + { + for (const nav_test_harness::ScenarioEvent& event : scenario_.events) + { + if (event.cycle != cycle) + { + continue; + } + if (event.action == "cancel") + { + loop_.requestCancel(); + } + else if (event.action == "pause") + { + loop_.requestPause(); + } + else if (event.action == "resume") + { + loop_.requestResume(); + } + else if (event.action == "lose_pose") + { + pose_.setAvailable(false); + } + else if (event.action == "restore_pose") + { + pose_.setAvailable(true); + } + } + } + + nav_test_harness::Scenario scenario_; + + ControlLoop loop_; + ControlLoopDeps deps_; + FakeClockPort clock_; + FakePosePort pose_; + FakePlannerPort planner_; + FakeControllerPort controller_; + FakeRecoveryPort recovery_{ 2 }; + FakeMissionPort mission_; + FakeActionPort action_; + + std::size_t cycle_ = 0; + bool started_ = false; +}; + +} // namespace testing +} // namespace move_base2 + +#endif // MOVE_BASE2_TEST_SCENARIO_DRIVER_H_ diff --git a/test/move_base2_scenario_test.cpp b/test/move_base2_scenario_test.cpp new file mode 100644 index 0000000..96885df --- /dev/null +++ b/test/move_base2_scenario_test.cpp @@ -0,0 +1,115 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * move_base2 — chạy toàn bộ kịch bản khai báo trong nav_test_harness/scenarios. + * + * Test này **tự tìm** file trong thư mục kịch bản và sinh một ca gtest cho mỗi file. Đó là điều kiện + * để giữ đúng mục tiêu của Phase 5: thêm một ca test = thêm một file YAML, không sửa C++. + * + * Author: DuongTD + *********************************************************************/ +#include + +#include +#include +#include + +#include +#include + +#include "move_base2_scenario_driver.h" + +namespace +{ +using move_base2::testing::MoveBase2ScenarioDriver; +using nav_test_harness::Scenario; +using nav_test_harness::ScenarioReport; +using nav_test_harness::ScenarioRunner; + +/// @brief Thư mục kịch bản, do CMake truyền vào — test không được đoán đường dẫn. +std::string scenarioDir() +{ + return MOVE_BASE2_SCENARIO_DIR; +} + +std::vector scenarioFiles() +{ + std::vector files = nav_test_harness::listScenarioFiles(scenarioDir()); + // Sắp xếp để thứ tự chạy ổn định giữa các máy — thứ tự đọc thư mục không được đảm bảo. + std::sort(files.begin(), files.end()); + return files; +} + +/// @brief Nạp, chạy, và khẳng định một kịch bản. +void runScenarioFile(const std::string& path) +{ + Scenario scenario; + std::string error; + ASSERT_TRUE(nav_test_harness::loadScenarioFile(path, scenario, error)) + << "không nạp được " << path << ": " << error; + + MoveBase2ScenarioDriver driver; + ASSERT_TRUE(driver.setup(scenario, error)) << scenario.name << ": setup thất bại: " << error; + + ScenarioRunner runner; + const ScenarioReport report = runner.run(scenario, driver); + + EXPECT_TRUE(report.passed) << nav_test_harness::formatReport(report); +} + +/** + * @class ScenarioFixture + * @brief Một ca gtest cho mỗi file kịch bản, tên ca lấy từ tên file. + */ +class ScenarioFixture : public ::testing::TestWithParam +{ +}; + +TEST_P(ScenarioFixture, Passes) +{ + runScenarioFile(GetParam()); +} + +/// @brief Tên ca test lấy từ tên file, bỏ đuôi và ký tự không hợp lệ. +std::string caseName(const ::testing::TestParamInfo& info) +{ + std::string name = info.param; + const std::size_t slash = name.find_last_of('/'); + if (slash != std::string::npos) + { + name = name.substr(slash + 1); + } + const std::size_t dot = name.find_last_of('.'); + if (dot != std::string::npos) + { + name = name.substr(0, dot); + } + for (char& c : name) + { + if (!std::isalnum(static_cast(c))) + { + c = '_'; + } + } + return name; +} + +INSTANTIATE_TEST_SUITE_P(Scenarios, ScenarioFixture, ::testing::ValuesIn(scenarioFiles()), + caseName); + +/// @brief Thư mục kịch bản không được rỗng — rỗng thì mọi ca ở trên biến mất mà suite vẫn xanh. +TEST(ScenarioSuite, ScenarioDirectoryIsNotEmpty) +{ + const std::vector files = scenarioFiles(); + EXPECT_FALSE(files.empty()) << "không tìm thấy kịch bản nào trong " << scenarioDir() + << " — suite sẽ xanh mà không kiểm gì cả"; +} + +} // namespace + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/navigation_server_test.cpp b/test/navigation_server_test.cpp index 8a33f36..5715756 100644 --- a/test/navigation_server_test.cpp +++ b/test/navigation_server_test.cpp @@ -9,10 +9,13 @@ *********************************************************************/ #include +#include +#include #include #include #include #include +#include #include #include @@ -236,8 +239,13 @@ 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. + // Stamp chỉ tiến khi đang có yêu cầu (xem test StampFreezesWhenIdle...), nên phải có goal chạy. Fixture fixture; fixture.configure(); + fixture.controller_.setNominalSpeed(0.3); // [m/s] + fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk }); + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); fixture.spin(1); EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart, 1e-9); @@ -247,6 +255,49 @@ TEST(NavigationServerTwist, StampComesFromTheControlLoopClockNotWallClock) EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), kClockStart + 12.0, 1e-9); } +TEST(NavigationServerTwist, StampFreezesWhenIdleSoTeleopOwnsCmdVel) +{ + // Không có yêu cầu nào thì stamp phải ĐỨNG YÊN dù control loop vẫn chạy: host publish /cmd_vel + // qua cửa tươi 0.05 s (amr_publiser.cpp:361), stamp tươi mỗi cycle nghĩa là amr_node phát 0 ở + // 20 Hz vĩnh viễn và đè chết teleop/joystick (rqt_robot_steering 10 Hz — robot chỉ nhích rồi + // đứng im). Bản cũ chỉ đóng dấu trong executeCycle; đây là regression đã gặp trên sim. + Fixture fixture; + fixture.configure(); + + fixture.spin(3); + EXPECT_TRUE(fixture.server_.getTwist().header.stamp.isZero()) + << "chưa từng có yêu cầu mà stamp đã tươi — host sẽ phát 0 đè teleop"; +} + +TEST(NavigationServerTwist, StampKeepsFreshBrieflyAfterGoalEndsThenFreezes) +{ + // Lệnh 0 cuối cùng phải qua được cửa 0.05 s của host — kết thúc mà đóng băng stamp ngay thì + // robot giữ nguyên vận tốc chót. Cửa ân hạn 0.5 s; hết ân hạn stamp phải đứng yên trả /cmd_vel + // cho teleop. + Fixture fixture; + fixture.configure(); + fixture.controller_.setNominalSpeed(0.3); // [m/s] + fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kGoalReached }); + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + + fixture.spin(4); // IDLE -> PLANNING -> CONTROLLING -> tới đích (terminal) + ASSERT_FALSE(fixture.server_.loop().hasActiveRequest()); + + // Ngay sau khi kết thúc: còn trong ân hạn, stamp vẫn tiến để host phát lệnh dừng. + const double stamp_in_grace = fixture.server_.getTwist().header.stamp.toSec(); + fixture.spin(1); + EXPECT_GT(fixture.server_.getTwist().header.stamp.toSec(), stamp_in_grace) + << "stamp đóng băng ngay khi kết thúc — lệnh dừng cuối không bao giờ được publish"; + + // Chạy qua hết cửa ân hạn (0.5 s = 10 cycle) rồi thêm vài cycle: stamp phải đứng yên. + fixture.spin(12); + const double stamp_frozen = fixture.server_.getTwist().header.stamp.toSec(); + fixture.spin(3); + EXPECT_NEAR(fixture.server_.getTwist().header.stamp.toSec(), stamp_frozen, 1e-9) + << "hết ân hạn mà stamp vẫn tươi — teleop không bao giờ lấy lại được /cmd_vel"; +} + TEST(NavigationServerTwist, StampStaysStillWhenTheControlLoopStopsRunning) { Fixture fixture; @@ -552,6 +603,257 @@ TEST(NavigationServerLimits, OdometryReachesTheControllerAsMeasuredVelocity) EXPECT_NEAR(fixture.controller_.measuredVelocity().angular.z, -0.17, 1e-9); } +// ================================================================================================ +// pause / resume / cancel — host gọi từ thread khác +// +// OPC-UA và VDA5050 chạy thread riêng (amr_control.cpp:159, 184) và gọi thẳng ba hàm này. +// `ControlLoop` tự khai là không thread-safe, và `requestPause()` còn ghi HAI cờ không nguyên tử — +// xen kẽ với `requestResume()` có thể để lại cả hai cùng false. Nên chúng chỉ được ghi nhận ở đây, +// rồi chuyển xuống lõi trên control thread. +// ================================================================================================ + +TEST(NavigationServerLifecycle, PauseTakesEffectOnTheNextCycleNotImmediately) +{ + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk, ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + fixture.spin(2); + ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling); + + fixture.server_.pause(); + EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling) + << "pause() đi thẳng vào lõi từ thread host"; + + fixture.spin(1); + EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kPaused); +} + +TEST(NavigationServerLifecycle, ResumeAfterPauseReturnsToControlling) +{ + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + fixture.spin(2); + + fixture.server_.pause(); + fixture.spin(1); + ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused); + + fixture.server_.resume(); + fixture.spin(1); + EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling); +} + +TEST(NavigationServerLifecycle, PauseThenResumeWithinOneCycleEndsResumed) +{ + // Hai cờ đối nghịch được đặt dưới cùng một lock, nên lệnh sau luôn thắng lệnh trước — không có + // trạng thái "cả hai cùng false" như bản ghi hai cờ rời rạc. + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + fixture.spin(2); + ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling); + + fixture.server_.pause(); + fixture.server_.resume(); + fixture.spin(1); + + EXPECT_EQ(fixture.server_.loop().state(), NavigationState::kControlling); +} + +TEST(NavigationServerLifecycle, CancelWinsOverAPauseRequestedInTheSameCycle) +{ + // "Tạm dừng rồi huỷ" và "huỷ rồi tạm dừng" phải cho cùng kết quả: huỷ thắng. + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + fixture.spin(2); + ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kControlling); + + fixture.server_.pause(); + fixture.server_.cancel(); + fixture.spin(1); + + EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused) << "pause thắng cancel"; +} + +TEST(NavigationServerLifecycle, LifecycleRequestIsConsumedExactlyOnce) +{ + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + fixture.spin(2); + + fixture.server_.pause(); + fixture.spin(1); + ASSERT_EQ(fixture.server_.loop().state(), NavigationState::kPaused); + + // Không có lệnh mới: cờ đã bị tiêu thụ, các cycle sau không được tự tạm dừng lại lần nữa. + fixture.server_.resume(); + fixture.spin(3); + EXPECT_NE(fixture.server_.loop().state(), NavigationState::kPaused); +} + +// ================================================================================================ +// Control thread +// +// Contract `BaseNavigation` KHÔNG có hàm spin nào: host nạp plugin, gọi initialize(), rồi chỉ tương +// tác qua moveTo/getTwist/getFeedback. Runtime vì thế phải TỰ LÁI mình, đúng như bản cũ (thread +// planner + action server). Thiếu control thread thì goal nằm im trong chỗ chờ vĩnh viễn — không +// cycle nào chạy, không state nào đổi, và không log gì cả. Đó là lỗi đã thật sự xảy ra trên sim. +// ================================================================================================ + +TEST(NavigationServerControlThread, RunsCyclesWithoutAnyoneCallingSpinOnce) +{ + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + + // Ở đây KHÔNG gọi spin() của fixture: chính control thread phải đẩy state đi. + ASSERT_TRUE(fixture.server_.startControlThread(200.0)); // [Hz] + + bool left_idle = false; + for (int i = 0; i < 500 && !left_idle; ++i) + { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + left_idle = fixture.server_.loop().state() != NavigationState::kIdle; + } + fixture.server_.stopControlThread(); + + EXPECT_TRUE(left_idle) << "goal được nhận nhưng không cycle nào chạy — thiếu control thread"; +} + +TEST(NavigationServerControlThread, RefusesToStartBeforeTheLoopIsConfigured) +{ + NavigationServer server; + EXPECT_FALSE(server.startControlThread(20.0)); + EXPECT_FALSE(server.controlThreadRunning()); +} + +TEST(NavigationServerControlThread, RefusesNonPositiveFrequency) +{ + Fixture fixture; + fixture.configure(); + + EXPECT_FALSE(fixture.server_.startControlThread(0.0)); + EXPECT_FALSE(fixture.server_.startControlThread(-5.0)); +} + +TEST(NavigationServerControlThread, SecondStartIsRefusedAndStopIsIdempotent) +{ + Fixture fixture; + fixture.configure(); + + ASSERT_TRUE(fixture.server_.startControlThread(100.0)); + EXPECT_FALSE(fixture.server_.startControlThread(100.0)) << "khởi động thread thứ hai"; + + fixture.server_.stopControlThread(); + fixture.server_.stopControlThread(); // không được treo hay sập + EXPECT_FALSE(fixture.server_.controlThreadRunning()); +} + +TEST(NavigationServerControlThread, DestructorStopsTheThread) +{ + // Thread chạm loop_, runtime_ và sensors_ mỗi cycle; huỷ chúng khi thread còn sống là hỏng ở chỗ + // không truy được. + Fixture fixture; + fixture.configure(); + ASSERT_TRUE(fixture.server_.startControlThread(100.0)); + SUCCEED(); // destructor của fixture phải join, không treo +} + +// ================================================================================================ +// Dữ liệu hiển thị — host gọi từ BỐN ros::Timer khác nhau +// ================================================================================================ + +TEST(NavigationServerPlannerData, GettersDoNotShareMutableState) +{ + // `getGlobalData` và `getLocalData` mỗi cái được gọi từ HAI timer (costmap và plan). Ghi vào + // `global_data_`/`local_data_` dùng chung đã gây `std::bad_alloc` rồi hỏng heap và giết tiến trình. + Fixture fixture; + fixture.configure(); + fixture.spin(1); + + robot::move_base_core::PlannerDataOutput a = fixture.server_.getGlobalData(); + robot::move_base_core::PlannerDataOutput b = fixture.server_.getGlobalData(); + + a.plan.poses.clear(); + EXPECT_TRUE(b.plan.poses.empty() || !a.plan.poses.empty()) + << "hai lần gọi trả về cùng một vùng nhớ"; + EXPECT_NO_THROW({ (void)fixture.server_.getLocalData(); }); +} + +TEST(NavigationServerPlannerData, ConcurrentGettersDoNotCorruptEachOther) +{ + // Không chứng minh được không có race (cần ThreadSanitizer), nhưng chạy đúng hình dạng lời gọi + // của host: bốn thread cùng đọc trong khi control thread cùng ghi bộ đệm. + Fixture fixture; + fixture.configure(); + fixture.controller_.setScript({ ControllerScript::kOk }); + ASSERT_TRUE(fixture.server_.moveTo(makeRequest(3.0).goal, 0.15, 0.10)) + << fixture.server_.lastRejectReason(); + ASSERT_TRUE(fixture.server_.startControlThread(200.0)); + + std::atomic stop{ false }; + std::vector readers; + for (int i = 0; i < 4; ++i) + { + readers.emplace_back([&fixture, &stop, i]() { + while (!stop.load()) + { + if (i % 2 == 0) + { + (void)fixture.server_.getGlobalData(); + } + else + { + (void)fixture.server_.getLocalData(); + } + } + }); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + stop.store(true); + for (auto& t : readers) + { + t.join(); + } + fixture.server_.stopControlThread(); + + SUCCEED(); +} + +TEST(NavigationServerPlannerData, PlanIsStampedWithTheControlLoopClock) +{ + Fixture fixture; + fixture.configure(); + + fixture.clock_.setTime(kClockStart + 7.0); + fixture.spin(1); + + EXPECT_NEAR(fixture.server_.getGlobalData().plan.header.stamp.toSec(), kClockStart + 7.0, 1e-9) + << "plan mang dấu thời gian khác đồng hồ control loop — host sẽ coi là quá hạn và bỏ qua"; +} + int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/plugins/test_local_planner.cpp b/test/plugins/test_local_planner.cpp index c3d8769..915bce3 100644 --- a/test/plugins/test_local_planner.cpp +++ b/test/plugins/test_local_planner.cpp @@ -20,6 +20,7 @@ *********************************************************************/ #include #include +#include #include #include #include @@ -30,7 +31,8 @@ #include #include -#include +#include +#include namespace move_base2 { @@ -44,7 +46,7 @@ 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 +class TestLocalPlanner : public robot_nav_core2::LocalPlanner { public: enum class Behavior @@ -60,46 +62,56 @@ public: { } - void initialize(std::string name, tf3::BufferCore* /*tf*/, - robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override + void initialize(robot::NodeHandle& /*parent*/, const std::string& name, + std::shared_ptr /*tf*/, + robot_costmap_2d::Costmap2DROBOT* /*costmap*/) override { // Cố ý KHÔNG chạm tf hay costmap — xem chú thích đầu file. - name_ = std::move(name); + name_ = name; } - bool setPlan(const std::vector& plan) override + void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override { - return !plan.empty(); + saw_goal_ = true; } - void getPlan(std::vector& path) override + void setPlan(const robot_nav_2d_msgs::Path2D& path) override { - path.clear(); + plan_size_ = path.poses.size(); } - void getGlobalPlan(std::vector& path) override + void getPlan(robot_nav_2d_msgs::Path2D& path) override { - path.clear(); + path = robot_nav_2d_msgs::Path2D(); } - bool computeVelocityCommands(const robot_geometry_msgs::Twist& velocity, - robot_geometry_msgs::Twist& cmd_vel) override + void getGlobalPlan(robot_nav_2d_msgs::Path2D& path) override { + path = robot_nav_2d_msgs::Path2D(); + } + + robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands( + const robot_nav_2d_msgs::Pose2DStamped& /*pose*/, + const robot_nav_2d_msgs::Twist2D& velocity) override + { + robot_nav_2d_msgs::Twist2DStamped cmd; + switch (behavior_) { case Behavior::kThrow: throw std::runtime_error("TestLocalPlanner được yêu cầu ném exception"); case Behavior::kNoCommand: - return false; + // Gen-2 không có cờ thành công/thất bại: "không sinh được lệnh" biểu đạt bằng exception. + throw std::runtime_error("TestLocalPlanner: không sinh được lệnh"); case Behavior::kNaN: - cmd_vel.linear.x = std::numeric_limits::quiet_NaN(); - return true; + cmd.velocity.x = std::numeric_limits::quiet_NaN(); + return cmd; case Behavior::kOk: case Behavior::kRefusesLimits: break; } - double linear = kBaseSpeed + velocity.linear.x; + double linear = kBaseSpeed + velocity.x; if (has_limit_forward_) { linear = std::min(linear, limit_forward_); @@ -111,12 +123,13 @@ public: yaw = std::min(yaw, limit_angular_); } - cmd_vel.linear.x = linear; - cmd_vel.angular.z = yaw; - return true; + cmd.velocity.x = linear; + cmd.velocity.theta = yaw; + return cmd; } - bool isGoalReached() override + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& /*pose*/, + const robot_nav_2d_msgs::Twist2D& /*velocity*/) override { return false; } @@ -168,6 +181,8 @@ public: private: Behavior behavior_; std::string name_; + std::size_t plan_size_ = 0; + bool saw_goal_ = false; double limit_forward_ = 0.0; ///< [m/s] double limit_backward_ = 0.0; ///< [m/s], âm double limit_angular_ = 0.0; ///< [rad/s] @@ -175,32 +190,32 @@ private: bool has_limit_angular_ = false; }; -robot_nav_core::BaseLocalPlanner::Ptr createOk() +robot_nav_core2::LocalPlanner::Ptr createOk() { return std::make_shared(TestLocalPlanner::Behavior::kOk); } -robot_nav_core::BaseLocalPlanner::Ptr createSecondary() +robot_nav_core2::LocalPlanner::Ptr createSecondary() { return std::make_shared(TestLocalPlanner::Behavior::kOk); } -robot_nav_core::BaseLocalPlanner::Ptr createNoCommand() +robot_nav_core2::LocalPlanner::Ptr createNoCommand() { return std::make_shared(TestLocalPlanner::Behavior::kNoCommand); } -robot_nav_core::BaseLocalPlanner::Ptr createNaN() +robot_nav_core2::LocalPlanner::Ptr createNaN() { return std::make_shared(TestLocalPlanner::Behavior::kNaN); } -robot_nav_core::BaseLocalPlanner::Ptr createThrowing() +robot_nav_core2::LocalPlanner::Ptr createThrowing() { return std::make_shared(TestLocalPlanner::Behavior::kThrow); } -robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits() +robot_nav_core2::LocalPlanner::Ptr createRefusingLimits() { return std::make_shared(TestLocalPlanner::Behavior::kRefusesLimits); } diff --git a/test/walking_skeleton_test.cpp b/test/walking_skeleton_test.cpp index bc8d714..b611c47 100644 --- a/test/walking_skeleton_test.cpp +++ b/test/walking_skeleton_test.cpp @@ -1002,6 +1002,115 @@ TEST(ControlLoopAsyncPlanner, FailureToStartAPlanIsTreatedAsAFailedAttempt) << join(fixture.states()); } +// ================================================================================================ +// Preempt — goal mới thay goal cũ NGAY +// +// Bấm goal mới nghĩa là goal cũ không còn muốn nữa. Xếp hàng chờ robot đi hết chặng cũ là hành vi +// không ai mong đợi, và mission layer cũng đã chốt "preempt ngay" (Q1, Phase 2). +// ================================================================================================ + +TEST(ControlLoopPreempt, NewGoalWhileControllingReplansImmediately) +{ + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + fixture.stepOnce(); // IDLE -> PLANNING + fixture.stepOnce(); // -> CONTROLLING + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + + const std::size_t plans_before = fixture.planner_.makePlanCount(); + + ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason; + fixture.stepOnce(); + + EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning) + << "goal mới nằm chờ thay vì thay goal cũ ngay"; + EXPECT_GT(fixture.planner_.makePlanCount(), plans_before) << "không lập plan lại cho goal mới"; +} + +TEST(ControlLoopPreempt, NewGoalWhilePlanningReplansImmediately) +{ + Fixture fixture; + fixture.planner_.setLatencyCycles(50); // plan cũ còn lâu mới xong + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + fixture.stepOnce(); + ASSERT_EQ(fixture.loop_.state(), NavigationState::kPlanning); + + ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason; + fixture.stepOnce(); + + EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning); + EXPECT_GE(fixture.planner_.cancelCount(), 1u) << "lượt lập plan của goal cũ không bị huỷ"; +} + +TEST(ControlLoopPreempt, OldMissionIsReportedPreemptedExactlyOnceUnderItsOwnId) +{ + // Bất biến quan trọng nhất: báo kết quả ĐÚNG MỘT LẦN và ĐÚNG ID. Preempt báo kết quả chặng cũ và + // nhận chặng mới trong cùng một cycle — dùng nhầm id thì mission layer mất dấu cả hai chặng. + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason; + fixture.stepOnce(); + fixture.stepOnce(); + ASSERT_EQ(fixture.loop_.state(), NavigationState::kControlling); + + ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason; + fixture.stepOnce(); + + EXPECT_EQ(fixture.mission_.reportCountFor(11), 1u) << "chặng bị thay không được báo đúng một lần"; + EXPECT_EQ(fixture.mission_.reportCountFor(22), 0u) << "chặng MỚI bị báo kết quả ngay khi nhận"; +} + +TEST(ControlLoopPreempt, PreemptedGoalStillFinishesTheNewOne) +{ + // Preempt không được để lại trạng thái nửa vời: chặng mới phải chạy tới cùng như bình thường. + Fixture fixture; + fixture.controller_.setScript({ControllerScript::kOk, ControllerScript::kOk, + ControllerScript::kGoalReached}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0, 11), reason)) << reason; + fixture.stepOnce(); + fixture.stepOnce(); + + ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0, 22), reason)) << reason; + fixture.run(); + + EXPECT_FALSE(fixture.hitLimit()) << join(fixture.states()); + EXPECT_STREQ(fixture.loop_.lastOutcome(), "SUCCEEDED"); + EXPECT_EQ(fixture.mission_.reportCountFor(22), 1u); +} + +TEST(ControlLoopPreempt, NewGoalDuringRecoveryCancelsTheRunningBehavior) +{ + ControlLoopConfig config = baseConfig(); + config.state_machine.planner_patience = 0.05; // [s] vào recovery nhanh + Fixture fixture(config); + fixture.planner_.setScript({PlannerScript::kFail}); + fixture.recovery_.setScript({RecoveryScript::kRunning}); + + std::string reason; + ASSERT_TRUE(fixture.loop_.submit(makeRequest(3.0), reason)) << reason; + for (int i = 0; i < 6 && fixture.loop_.state() != NavigationState::kRecovering; ++i) + { + fixture.stepOnce(); + } + ASSERT_EQ(fixture.loop_.state(), NavigationState::kRecovering) << join(fixture.states()); + + fixture.planner_.setScript({PlannerScript::kOk}); + ASSERT_TRUE(fixture.loop_.submit(makeRequest(9.0), reason)) << reason; + fixture.stepOnce(); + + EXPECT_EQ(fixture.loop_.state(), NavigationState::kPlanning); + EXPECT_GE(fixture.recovery_.cancelCount(), 1u) << "behavior đang chạy không được bảo dừng"; +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);