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

optimal

Browse Source
This commit is contained in:
duongtd
2026-07-30 09:24:47 +07:00
parent 4762a3032c
commit d8babff20b
50 changed files with 3280 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

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

1
config/runtime/config Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/config

1
config/runtime/costmap_common_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/costmap_common_params.yaml

1
config/runtime/costmap_global_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/costmap_global_params.yaml

1
config/runtime/costmap_global_params_plugins_no_virtual_walls.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/costmap_global_params_plugins_no_virtual_walls.yaml

1
config/runtime/costmap_local_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/costmap_local_params.yaml

1
config/runtime/costmap_local_params_plugins_no_virtual_walls.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/costmap_local_params_plugins_no_virtual_walls.yaml

1
config/runtime/custom_global_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/custom_global_params.yaml

1
config/runtime/dock_global_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/dock_global_params.yaml

1
config/runtime/hybrid_local_planner_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/hybrid_local_planner_params.yaml

1
config/runtime/maker_sources.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/maker_sources.yaml

1
config/runtime/mission_adapters_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/mission_adapters_params.yaml

60
config/runtime/move_base_common_params.yaml Normal file
View File

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

1
config/runtime/mppi_local_planner_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/mppi_local_planner_params.yaml

1
config/runtime/mprim Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/mprim

1
config/runtime/pnkx_local_planner_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/pnkx_local_planner_params.yaml

1
config/runtime/priest_local_planner_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/priest_local_planner_params.yaml

1
config/runtime/recovery_behaviors_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/recovery_behaviors_params.yaml

1
config/runtime/sbpl_global_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/sbpl_global_params.yaml

1
config/runtime/stanley_local_planner_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/stanley_local_planner_params.yaml

1
config/runtime/two_points_global_params.yaml Symbolic link
View File

@@ -0,0 +1 @@
../../../../pnkx_nav_core/config/two_points_global_params.yaml

12
include/move_base2/control_loop.h
View File

@@ -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<robot_geometry_msgs::PoseStamped>& 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;

20
include/move_base2/core/state_machine.h
View File

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

91
include/move_base2/io/costmap_exporter.h Normal file
View File

@@ -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 <mutex>
#include <string>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <robot_nav_msgs/OccupancyGrid.h>
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_

229
include/move_base2/navigation_runtime.h Normal file
View File

@@ -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 <memory>
#include <string>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <move_base2/bridges/mission_adapter_bridge.h>
#include <move_base2/config/move_base2_config.h>
#include <move_base2/control_loop.h>
#include <move_base2/io/costmap_exporter.h>
#include <move_base2/ports/clock_port.h>
#include <move_base2/ports/pose_port.h>
#include <move_base2/runners/action_runner.h>
#include <move_base2/runners/controller_runner.h>
#include <move_base2/runners/planner_runner.h>
#include <move_base2/runners/recovery_runner.h>
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<tf3::BufferCore>& 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<tf3::BufferCore> 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<robot_costmap_2d::Costmap2DROBOT> global_costmap_;
std::unique_ptr<robot_costmap_2d::Costmap2DROBOT> 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_

100
include/move_base2/navigation_server.h
View File

@@ -9,9 +9,11 @@
#ifndef MOVE_BASE2_NAVIGATION_SERVER_H_
#define MOVE_BASE2_NAVIGATION_SERVER_H_
#include <atomic>
#include <map>
#include <memory>
#include <mutex>
#include <thread>
#include <string>
#include <vector>
@@ -20,6 +22,7 @@
#include <move_base2/control_loop.h>
#include <move_base2/core/navigation_request.h>
#include <move_base2/io/sensor_gateway.h>
#include <move_base2/navigation_runtime.h>
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<robot_geometry_msgs::Point>& fprt) override;
std::vector<robot_geometry_msgs::Point> 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<NavigationRuntime> runtime_;
/// Control thread — thread DUY NHẤT chạy control loop và phát cmd_vel.
std::thread control_thread_;
std::atomic<bool> 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_;
};

12
include/move_base2/ports/controller_port.h
View File

@@ -15,6 +15,7 @@
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_nav_2d_msgs/Path2D.h>
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.
*

55
include/move_base2/runners/controller_runner.h
View File

@@ -11,14 +11,17 @@
#include <cstddef>
#include <functional>
#include <memory>
#include <map>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <robot_nav_core/base_local_planner.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/local_planner.h>
#include <move_base2/ports/controller_port.h>
#include <move_base2/ports/pose_port.h>
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<BaseLocalPlanner::Ptr()>` 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<tf3::BufferCore>& 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<robot_geometry_msgs::PoseStamped>& 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<robot_nav_core::BaseLocalPlanner::Ptr()> factory;
robot_nav_core::BaseLocalPlanner::Ptr instance;
std::function<robot_nav_core2::LocalPlanner::Ptr()> 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<tf3::BufferCore> tf_;
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
const PosePort* pose_ = nullptr; ///< Non-owning.
bool configured_ = false;
std::map<std::string, Loaded> controllers_;
std::string active_name_;
robot_nav_core::BaseLocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_.
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.

110
launch/README.md Normal file
View File

@@ -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: <toàn bộ config>
```
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: <lý do>
```
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`.

66
launch/move_base2_control.launch Normal file
View File

@@ -0,0 +1,66 @@
<?xml version="1.0"?>
<!--
Nav stack chạy trên move_base2. Bản độc lập, viết theo mẫu `amr_startup/launch/amr_control.launch`
nhưng KHÔNG include nó — để đổi thứ gì ở đây cũng không đụng vào đường chạy của move_base cũ.
Khác biệt so với bản gốc đúng hai chỗ:
1. `PNKX_NAV_CORE_CONFIG_DIR` trỏ vào cây overlay của move_base2 (xem config/runtime/README).
2. Không cần `export` thủ công nữa — launch tự đặt.
Mọi tham số PHÍA ROS (footprint, costmap, localization, mqtt) vẫn đọc từ `amr_startup/config`,
đúng như bản gốc: đó là cây config của phía ROS, không phải của nav core.
-->
<launch>
<arg name="robot_type" default="T800" doc="T800 | imr | hook_150 | APM" />
<arg name="local_planner" default="pnkx" doc="dwa | dwb | eband | base | teb | pose | pnkx" />
<arg name="global_planner" default="custom" doc="sbpl | two_points | custom" />
<arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path | vda5050_msgs::Order" />
<arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim" />
<arg name="prefix" default="" doc="Prefix cho tf frame của robot" />
<!--
ĐIỂM KHÁC BIỆT DUY NHẤT của cả đường chạy: nav core đọc config từ đây.
Cây overlay là symlink tới toàn bộ `pnkx_nav_core/config/`, trừ đúng một file thật
`move_base_common_params.yaml` với `MoveBase: library_path: libmove_base2`.
-->
<env name="PNKX_NAV_CORE_CONFIG_DIR" value="$(find move_base2)/config/runtime" />
<param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config" />
<rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
<node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
<rosparam param="footprint" if="$(eval robot_type == 'imr')">
[[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
</rosparam>
<rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
[[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
</rosparam>
<rosparam param="footprint" if="$(eval robot_type == 'T800')">
[[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
</rosparam>
<rosparam param="footprint" if="$(eval robot_type == 'APM')">
[[1.65,-0.4],[1.65,0.4],[-0.35,0.4],[-0.35,-0.4]]
</rosparam>
<param name="primitive_filename" value="$(arg primitive_filename)" />
<param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
<rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
<remap from="map" to="/map" />
<remap from="marker" to="move_base_node/markers" />
</node>
</launch>

54
launch/move_base2_sim.launch Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0"?>
<!--
Một lệnh duy nhất thay cho năm lệnh của đường chạy cũ:
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=.../pnkx_nav_core/config
roslaunch amr_startup amr_control.launch
Ba phần đầu là hạ tầng mô phỏng KHÔNG đổi giữa move_base cũ và move_base2 (cùng world, cùng model
URDF T800, cùng rviz, cùng depth proc) nên được include lại chứ không chép lại — chép lại chỉ tạo
thêm một bản sao phải đồng bộ bằng tay.
Phần thứ tư và năm là thứ thực sự khác, và nằm trong `move_base2_control.launch` của gói này.
-->
<launch>
<arg name="world" default="maze" doc="maze | warehouse | sehc | empty" />
<arg name="robot_type" default="T800" />
<arg name="gui" default="true" doc="false = chạy gzserver headless" />
<arg name="rviz" default="true" />
<arg name="depth_proc" default="true" doc="Bật khi cần đường depth camera -> costmap" />
<!-- 1. Gazebo + model robot T800 + world -->
<include file="$(find amr_startup)/launch/robot_maze_world.launch" if="$(eval world == 'maze')">
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
</include>
<include file="$(find amr_startup)/launch/ware_houses_world.launch" if="$(eval world == 'warehouse')">
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
</include>
<include file="$(find amr_startup)/launch/sehc_world.launch" if="$(eval world == 'sehc')">
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
</include>
<include file="$(find amr_startup)/launch/robot_empty_world.launch" if="$(eval world == 'empty')">
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
</include>
<!-- 2. rviz — dùng lại đúng config đang đọc /global_costmap/costmap, /plan, /footprint -->
<node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz"
args="-d $(find amr_startup)/rviz/navigation.rviz" />
<!-- 3. Depth image -> point cloud cho VoxelLayer -->
<include file="$(find robot_depth_image_proc)/launch/depth_image_proc_gazebo.launch"
if="$(arg depth_proc)" />
<!-- 4 + 5. Nav stack trên move_base2 (tự đặt PNKX_NAV_CORE_CONFIG_DIR) -->
<include file="$(find move_base2)/launch/move_base2_control.launch">
<arg name="robot_type" value="$(arg robot_type)" />
</include>
</launch>

3
package.xml
View File

@@ -70,7 +70,8 @@
<run_depend>yaml-cpp</run_depend>
<!-- Chỉ dùng cho test: bộ fake và scenario runner dùng chung. -->
<test_depend>nav_test_harness</test_depend>
<build_depend>nav_test_harness</build_depend>
<run_depend>nav_test_harness</run_depend>
<build_depend>recovery_core</build_depend>
<run_depend>recovery_core</run_depend>

182
src/config/move_base2_config.cpp
View File

@@ -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á `<profile>_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;

64
src/control_loop.cpp
View File

@@ -11,6 +11,8 @@
#include <cmath>
#include <sstream>
#include <robot/console.h>
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<unsigned long long>(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<unsigned long long>(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<unsigned long long>(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<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kCancelled:
robot::log_info("[move_base2] Goal bị huỷ (mission %llu: CANCELLED).\n",
static_cast<unsigned long long>(outgoing_mission_id));
break;
case NavigationOutcome::kFailed:
robot::log_error("[move_base2] Navigation thất bại (mission %llu: ABORTED): %s\n",
static_cast<unsigned long long>(outgoing_mission_id),
last_reason_ != nullptr ? last_reason_ : "");
break;
}
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;
cancel_requested_ = false;
latest_plan_.clear();
planner_running_ = false;
}
cancel_requested_ = 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

148
src/io/costmap_exporter.cpp Normal file
View File

@@ -0,0 +1,148 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt CostmapExporter.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/io/costmap_exporter.h>
#include <array>
#include <boost/thread/locks.hpp>
#include <robot/robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/layered_costmap.h>
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<signed char, 256>& costTranslationTable()
{
static const std::array<signed char, 256> table = [] {
std::array<signed char, 256> t{};
// Dải thường: 10..252 của costmap co về 1..98 của OccupancyGrid.
for (int i = static_cast<int>(robot_costmap_2d::FREE_SPACE) + 1;
i < static_cast<int>(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); ++i)
{
t[static_cast<std::size_t>(i)] = static_cast<signed char>(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<std::size_t>(robot_costmap_2d::PREFERRED_SPACE)] = 120;
for (int i = static_cast<int>(robot_costmap_2d::PREFERRED_SPACE) + 1;
i <= static_cast<int>(robot_costmap_2d::FREE_SPACE); ++i)
{
t[static_cast<std::size_t>(i)] = 0; // không có vật cản
}
t[static_cast<std::size_t>(robot_costmap_2d::CRITICAL_SPACE)] = 150;
t[static_cast<std::size_t>(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)] = 99;
t[static_cast<std::size_t>(robot_costmap_2d::LETHAL_OBSTACLE)] = 100;
t[static_cast<std::size_t>(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<std::mutex> 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<std::size_t>(grid_.info.width) * grid_.info.height);
const unsigned char* data = costmap->getCharMap();
const std::array<signed char, 256>& 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<std::mutex> 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<robot_costmap_2d::Costmap2D::mutex_t> 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

13
src/move_base2_plugin.cpp
View File

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

202
src/navigation_runtime.cpp Normal file
View File

@@ -0,0 +1,202 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* move_base2 — cài đặt NavigationRuntime.
*
* Author: DuongTD
*********************************************************************/
#include <move_base2/navigation_runtime.h>
#include <exception>
#include <robot/node_handle.h>
#include <robot/robot.h>
#include <tf3/buffer_core.h>
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<tf3::BufferCore>& 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

327
src/navigation_server.cpp
View File

@@ -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<std::mutex> lock(data_mutex_);
twist_.velocity = robot_nav_2d_utils::twist3Dto2D(command);
twist_.header.stamp = loop_.lastCycleTime();
twist_.header.frame_id = robot_base_frame_;
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<unsigned long long>(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<std::mutex> 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<std::mutex> lock(data_mutex_);
resume_requested_ = true;
pause_requested_ = false;
}
void NavigationServer::cancel()
{
std::lock_guard<std::mutex> lock(data_mutex_);
cancel_requested_ = true;
}
void NavigationServer::drainLifecycleRequests()
{
bool pause = false;
bool resume = false;
bool cancel = false;
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<float>(point.x);
p32.y = static_cast<float>(point.y);
p32.z = static_cast<float>(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

137
src/runners/controller_runner.cpp
View File

@@ -17,6 +17,8 @@
#include <robot/plugin_loader_helper.h>
#include <robot/robot.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_nav_2d_utils/conversions.h>
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<tf3::BufferCore>& 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<robot_nav_core::BaseLocalPlanner::Ptr()>(
loaded.factory = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
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::vector<robot_geometry_msgs::PoseStampe
try
{
return active_->setPlan(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<Pose2DStamped>, 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::vector<robot_geometry_msgs::PoseStampe
}
}
bool ControllerRunner::currentPose(robot_nav_2d_msgs::Pose2DStamped& pose) const
{
if (pose_ == nullptr)
{
return false;
}
robot_geometry_msgs::PoseStamped stamped;
if (!pose_->getRobotPose(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))

126
src/state_machine.cpp
View File

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

24
test/config/move_base2_params.yaml
View File

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

99
test/config_validation_test.cpp
View File

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

40
test/controller_runner_test.cpp
View File

@@ -20,9 +20,25 @@
#include <move_base2/runners/controller_runner.h>
#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));

12
test/fake_ports.h
View File

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

287
test/mission_adapter_bridge_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <memory>
#include <string>
#include <vector>
#include <mission_adapters/mission_manager.h>
#include <mission_adapters/types.h>
#include <move_base2/bridges/mission_adapter_bridge.h>
namespace
{
using mission_adapters::Mission;
using mission_adapters::MissionManager;
using move_base2::MissionAdapterBridge;
using move_base2::NavigationOutcome;
using move_base2::NavigationRequest;
std::shared_ptr<Mission> 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>();
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<int>(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<NavigationRequest> 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();
}

294
test/move_base2_scenario_driver.h Normal file
View File

@@ -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 <cmath>
#include <cstddef>
#include <string>
#include <vector>
#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>
#include <move_base2/control_loop.h>
#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<PlannerScript> 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<ControllerScript> 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<RecoveryScript> 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_

115
test/move_base2_scenario_test.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#include <algorithm>
#include <string>
#include <vector>
#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>
#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<std::string> scenarioFiles()
{
std::vector<std::string> 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<std::string>
{
};
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<std::string>& 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<unsigned char>(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<std::string> 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();
}

302
test/navigation_server_test.cpp
View File

@@ -9,10 +9,13 @@
*********************************************************************/
#include <gtest/gtest.h>
#include <atomic>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <limits>
#include <string>
#include <thread>
#include <vector>
#include <robot_costmap_2d/layered_costmap.h>
@@ -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<bool> stop{ false };
std::vector<std::thread> 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);

69
test/plugins/test_local_planner.cpp
View File

@@ -20,6 +20,7 @@
*********************************************************************/
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <stdexcept>
@@ -30,7 +31,8 @@
#include <boost/config.hpp>
#include <boost/dll/alias.hpp>
#include <robot_nav_core/base_local_planner.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_core2/local_planner.h>
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<tf3::BufferCore> /*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<robot_geometry_msgs::PoseStamped>& plan) override
void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped& /*goal_pose*/) override
{
return !plan.empty();
saw_goal_ = true;
}
void getPlan(std::vector<robot_geometry_msgs::PoseStamped>& path) override
void setPlan(const robot_nav_2d_msgs::Path2D& path) override
{
path.clear();
plan_size_ = path.poses.size();
}
void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& 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<double>::quiet_NaN();
return true;
cmd.velocity.x = std::numeric_limits<double>::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>(TestLocalPlanner::Behavior::kOk);
}
robot_nav_core::BaseLocalPlanner::Ptr createSecondary()
robot_nav_core2::LocalPlanner::Ptr createSecondary()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kOk);
}
robot_nav_core::BaseLocalPlanner::Ptr createNoCommand()
robot_nav_core2::LocalPlanner::Ptr createNoCommand()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNoCommand);
}
robot_nav_core::BaseLocalPlanner::Ptr createNaN()
robot_nav_core2::LocalPlanner::Ptr createNaN()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kNaN);
}
robot_nav_core::BaseLocalPlanner::Ptr createThrowing()
robot_nav_core2::LocalPlanner::Ptr createThrowing()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kThrow);
}
robot_nav_core::BaseLocalPlanner::Ptr createRefusingLimits()
robot_nav_core2::LocalPlanner::Ptr createRefusingLimits()
{
return std::make_shared<TestLocalPlanner>(TestLocalPlanner::Behavior::kRefusesLimits);
}

109
test/walking_skeleton_test.cpp
View File

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