first commit
This commit is contained in:
142
include/move_base2/bridges/mission_adapter_bridge.h
Normal file
142
include/move_base2/bridges/mission_adapter_bridge.h
Normal file
@@ -0,0 +1,142 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — nối MissionPort với framework mission_adapters.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_
|
||||
#define MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
|
||||
#include <mission_adapters/navigation_client.h>
|
||||
#include <mission_adapters/types.h>
|
||||
|
||||
#include <move_base2/ports/mission_port.h>
|
||||
|
||||
namespace mission_adapters
|
||||
{
|
||||
class MissionManager;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class MissionAdapterBridge
|
||||
* @brief Lớp nối duy nhất giữa `move_base2` và `mission_adapters`.
|
||||
*
|
||||
* Đây là file **duy nhất** trong gói include `mission_adapters`. Lõi quyết định chỉ thấy
|
||||
* @ref MissionPort và không biết framework mission nào đang chạy phía sau.
|
||||
*
|
||||
* Bắc qua hai interface cùng lúc:
|
||||
* - @ref MissionPort — phía `move_base2` nhìn vào;
|
||||
* - `mission_adapters::NavigationClient` — phía mission layer nhìn vào.
|
||||
*
|
||||
* ## Biên thread nằm ở đây, có chủ đích
|
||||
*
|
||||
* `MissionExecutor` gọi `dispatch()` từ **thread của nó**, còn `ControlLoop` không thread-safe và
|
||||
* chỉ được chạm từ control thread. Nếu `dispatch()` gọi thẳng xuống navigation thì hai thread cùng
|
||||
* ghi `pending_request_` của control loop.
|
||||
*
|
||||
* Vì vậy `dispatch()` chỉ **cất mission lại** rồi trả về ngay (executor cũng không được phép block —
|
||||
* xem bảng threading của plan). Control thread gọi @ref pumpPendingRequest mỗi cycle; chính ở đó
|
||||
* mission mới được chuyển thành @ref NavigationRequest và đẩy qua callback.
|
||||
*
|
||||
* ## Vì sao chỉ giữ MỘT mission chờ
|
||||
*
|
||||
* `MissionManager::nextMission()` chỉ trả mỗi mission đúng một lần và chỉ giao chặng mới sau khi
|
||||
* chặng cũ kết thúc, nên trong thực tế không bao giờ có hai mission cùng chờ. Nếu vẫn xảy ra thì
|
||||
* mission mới đè mission cũ và việc đó được **đếm lại** (@ref droppedRequests) — im lặng ở đây
|
||||
* nghĩa là một chặng biến mất mà fleet master vẫn chờ nó.
|
||||
*/
|
||||
class MissionAdapterBridge : public MissionPort, public mission_adapters::NavigationClient
|
||||
{
|
||||
public:
|
||||
/// @brief Được gọi khi navigation cần dừng chặng đang chạy. Chạy trên **control thread**.
|
||||
using CancelCallback = std::function<void()>;
|
||||
|
||||
MissionAdapterBridge();
|
||||
~MissionAdapterBridge() override;
|
||||
|
||||
MissionAdapterBridge(const MissionAdapterBridge&) = delete;
|
||||
MissionAdapterBridge& operator=(const MissionAdapterBridge&) = delete;
|
||||
|
||||
/**
|
||||
* @brief Gắn mission manager để @ref reportOutcome có chỗ báo về.
|
||||
* @param manager **Non-owning**, được phép null (chạy không có mission layer).
|
||||
*/
|
||||
void attach(mission_adapters::MissionManager* manager);
|
||||
|
||||
/**
|
||||
* @brief Đăng ký cách dừng navigation.
|
||||
*
|
||||
* Không nằm trong @ref MissionPort vì đó là cổng một chiều theo thiết kế: mission đẩy chặng
|
||||
* xuống, navigation báo kết quả lên. Nhưng `NavigationClient::cancelActive` bắt buộc phải có
|
||||
* đường tác động ngược, nên nó đi qua callback riêng này.
|
||||
*/
|
||||
void setCancelCallback(CancelCallback callback);
|
||||
|
||||
/**
|
||||
* @brief Chuyển mission đang chờ (nếu có) thành yêu cầu và đẩy qua callback đã đăng ký.
|
||||
*
|
||||
* **Chỉ gọi từ control thread**, mỗi cycle một lần. Đây là chỗ duy nhất biên thread được vượt qua.
|
||||
*
|
||||
* @return true nếu có một yêu cầu vừa được đẩy xuống.
|
||||
*/
|
||||
bool pumpPendingRequest();
|
||||
|
||||
// ================================================================================================
|
||||
// MissionPort — phía move_base2
|
||||
// ================================================================================================
|
||||
|
||||
void setRequestCallback(RequestCallback callback) override;
|
||||
void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) override;
|
||||
bool hasActiveMission() const override;
|
||||
void start() override;
|
||||
void stop() override;
|
||||
|
||||
// ================================================================================================
|
||||
// mission_adapters::NavigationClient — phía mission layer
|
||||
// ================================================================================================
|
||||
|
||||
bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override;
|
||||
void cancelActive(mission_adapters::MissionId id) override;
|
||||
|
||||
// ================================================================================================
|
||||
// Chẩn đoán
|
||||
// ================================================================================================
|
||||
|
||||
/// @brief Số mission bị đè vì tới khi mission trước chưa kịp được đẩy xuống.
|
||||
std::size_t droppedRequests() const;
|
||||
|
||||
/// @brief Số lần báo kết quả mà mission layer không nhận (outcome tới trễ, mission đã bị thay).
|
||||
std::size_t staleOutcomes() const;
|
||||
|
||||
/// @brief Chuyển một mission thành yêu cầu navigation. Phơi ra để test được phần chuyển đổi.
|
||||
static NavigationRequest toRequest(const mission_adapters::Mission& mission);
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
|
||||
mission_adapters::MissionManager* manager_ = nullptr; ///< Non-owning, có thể null.
|
||||
RequestCallback request_callback_;
|
||||
CancelCallback cancel_callback_;
|
||||
|
||||
std::shared_ptr<const mission_adapters::Mission> pending_;
|
||||
bool running_ = false;
|
||||
|
||||
std::size_t dropped_requests_ = 0;
|
||||
std::size_t stale_outcomes_ = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_BRIDGES_MISSION_ADAPTER_BRIDGE_H_
|
||||
153
include/move_base2/config/move_base2_config.h
Normal file
153
include/move_base2/config/move_base2_config.h
Normal file
@@ -0,0 +1,153 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — toàn bộ tham số runtime, đọc từ YAML và validate ở một chỗ.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_
|
||||
#define MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
|
||||
#include <move_base2/control_loop.h>
|
||||
#include <move_base2/io/sensor_gateway.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct MoveBase2Config
|
||||
* @brief Cấu hình đầy đủ của navigation runtime.
|
||||
*
|
||||
* Ba tính chất bắt buộc, áp cho **mọi** tham số ở đây:
|
||||
* 1. có default ngay tại khai báo (không có magic number rải trong code);
|
||||
* 2. có đơn vị ghi tại chỗ khai báo;
|
||||
* 3. đi qua @ref validate — sai miền giá trị thì runtime **không khởi động**, thay vì chạy tiếp
|
||||
* với một giá trị vô nghĩa.
|
||||
*
|
||||
* Điểm (3) là khác biệt có chủ đích so với bản cũ: nó đọc param không kiểm miền, và có chỗ đọc
|
||||
* `max_planning_retries` vào `double` rồi gán sang `int32_t` — một giá trị âm hoặc quá lớn đi thẳng
|
||||
* vào vòng điều khiển mà không ai biết.
|
||||
*/
|
||||
struct MoveBase2Config
|
||||
{
|
||||
// --- Nhịp chạy -------------------------------------------------------------------------------
|
||||
|
||||
/// [Hz] Nhịp control loop. Đây cũng là nhịp tick recovery và action.
|
||||
double controller_frequency = 20.0;
|
||||
|
||||
/// [Hz] Nhịp lập plan lại khi đang bám plan. 0 = chỉ lập khi cần (theo yêu cầu của state machine).
|
||||
double planner_frequency = 0.0;
|
||||
|
||||
/// [s] Trần thời gian chờ một lần lập plan trước khi coi là hỏng. <= 0 = không giới hạn.
|
||||
double planner_timeout = 5.0;
|
||||
|
||||
// --- Hành vi chuyển state --------------------------------------------------------------------
|
||||
|
||||
StateMachineConfig state_machine;
|
||||
|
||||
// --- Hàng rào vận tốc cuối cùng ---------------------------------------------------------------
|
||||
|
||||
VelocityLimits velocity;
|
||||
|
||||
// --- Đường vào cảm biến -----------------------------------------------------------------------
|
||||
|
||||
/// Đọc từ namespace con `sensors`. Xem @ref SensorGatewayConfig::laser_sor_enabled về lý do
|
||||
/// bộ lọc laser mặc định tắt.
|
||||
SensorGatewayConfig sensors;
|
||||
|
||||
// --- Ánh xạ profile -> planner ----------------------------------------------------------------
|
||||
|
||||
ProfileBinding position;
|
||||
ProfileBinding docking;
|
||||
ProfileBinding go_straight;
|
||||
ProfileBinding rotate;
|
||||
|
||||
// --- Namespace cho các thành phần nạp plugin ---------------------------------------------------
|
||||
|
||||
/// Namespace chứa danh sách recovery behavior (`<ns>/behaviors`) trong YAML.
|
||||
std::string recovery_namespace = "recovery";
|
||||
|
||||
/// Namespace chứa danh sách action handler (`<ns>/handlers`) trong YAML.
|
||||
std::string action_namespace = "actions";
|
||||
|
||||
/// Namespace chứa cấu hình mission layer.
|
||||
std::string mission_namespace = "mission_adapters";
|
||||
|
||||
// --- Frame ------------------------------------------------------------------------------------
|
||||
|
||||
/// Frame mà goal được quy về trước khi lập plan.
|
||||
std::string global_frame = "map";
|
||||
|
||||
/// Frame gắn với thân robot.
|
||||
std::string robot_base_frame = "base_link";
|
||||
|
||||
/**
|
||||
* @brief Đọc toàn bộ tham số từ @p nh.
|
||||
*
|
||||
* Khoá thiếu thì giữ default và **log rõ khoá nào** — im lặng dùng default là cách một tham số
|
||||
* quan trọng biến mất mà không ai phát hiện. Hàm này không validate; gọi @ref validate sau.
|
||||
*
|
||||
* @param nh NodeHandle đã scope vào namespace của runtime.
|
||||
*/
|
||||
void fromNodeHandle(robot::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Đọc theo schema move_base gen-1 (`move_base_common_params.yaml`, khoá ở root).
|
||||
*
|
||||
* Cho phép chuyển sang move_base2 mà KHÔNG phải viết lại cây config đang chạy. Ánh xạ:
|
||||
* - `position/docking/go_straight/rotate_planner_name` -> `<profile>.local_planner_name`;
|
||||
* - `<TênPlanner>: base_global_planner` -> `<profile>.global_planner_name`, thiếu thì rơi về
|
||||
* `base_global_planner` ở root;
|
||||
* - `base_local_planner` (LocalPlannerAdapter) bị BỎ QUA có log: adapter là cầu nhúng planner
|
||||
* gen-2 vào move_base gen-1, move_base2 gọi thẳng interface gen-2 qua ControllerPort;
|
||||
* - `xy/yaw_goal_tolerance` ở root -> tolerance mặc định của cả bốn profile.
|
||||
*
|
||||
* Hai khác biệt NGỮ NGHĨA được dịch tường minh (có log cảnh báo khi kích hoạt):
|
||||
* 1. patience = 0: gen-1 nghĩa là "fail -> recovery NGAY" (mốc + 0 luôn ở quá khứ), gen-2 nghĩa
|
||||
* là "tắt đồng hồ". Giá trị <= 0 được dịch thành đúng MỘT chu kỳ điều khiển — gen-1 cũng chỉ
|
||||
* phản ứng được ở độ phân giải cycle nên hành vi giữ nguyên.
|
||||
* 2. default của gen-1 khác gen-2: `robot_base_frame` = "base_footprint" (gen-2: "base_link"),
|
||||
* tolerance = 0.2 (gen-2: 0.15/0.10). Chế độ legacy giữ default gen-1.
|
||||
*
|
||||
* @param nh NodeHandle nhìn thấy các khoá gen-1 (thường là root "~").
|
||||
*/
|
||||
void fromLegacyNodeHandle(robot::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Tự nhận diện schema rồi đọc: có namespace `move_base2` -> schema mới (khoá gen-1 nếu
|
||||
* còn nằm cạnh sẽ bị bỏ qua toàn bộ — KHÔNG trộn từng khoá giữa hai schema); không có
|
||||
* nhưng thấy khoá gen-1 -> @ref fromLegacyNodeHandle; không thấy gì -> default + log.
|
||||
*
|
||||
* Không trộn per-key là chủ đích: hai nguồn cùng có hiệu lực cho một tham số là đúng kiểu lỗi
|
||||
* "sửa config mãi không ăn" đã ghi nhận với hai cây config trùng tên của workspace.
|
||||
*/
|
||||
static MoveBase2Config load(robot::NodeHandle& root_nh);
|
||||
|
||||
/**
|
||||
* @brief Kiểm miền giá trị của mọi tham số, gồm cả các struct con.
|
||||
* @param[out] error Mô tả tham số sai đầu tiên gặp phải; chỉ ghi khi hàm trả false.
|
||||
*
|
||||
* @warning **Ràng buộc thứ tự khởi tạo:** `state_machine.recovery_behavior_count` không đến từ
|
||||
* YAML mà là số behavior `RecoveryRunner` nạp được **thật**. Trình tự đúng là:
|
||||
* `fromNodeHandle()` → `RecoveryRunner::configure()` → gán
|
||||
* `state_machine.recovery_behavior_count = runner.behaviorCount()` → `validate()`.
|
||||
* Đọc con số này từ YAML thì một behavior hỏng vẫn khiến state machine tin là còn đường
|
||||
* phục hồi, và lỗi đầu tiên sẽ dẫn thẳng tới ABORTED mà không ai hiểu vì sao.
|
||||
*/
|
||||
bool validate(std::string& error) const;
|
||||
|
||||
/// @brief Kết xuất thành text nhiều dòng, để log đúng một lần lúc khởi tạo.
|
||||
std::string describe() const;
|
||||
|
||||
/// @brief Phần cấu hình mà @ref ControlLoop cần, trích ra từ bản đầy đủ này.
|
||||
ControlLoopConfig toControlLoopConfig() const;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CONFIG_MOVE_BASE2_CONFIG_H_
|
||||
292
include/move_base2/control_loop.h
Normal file
292
include/move_base2/control_loop.h
Normal file
@@ -0,0 +1,292 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — một control cycle: gom dữ liệu, chạy state machine, thi hành kết quả.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CONTROL_LOOP_H_
|
||||
#define MOVE_BASE2_CONTROL_LOOP_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
|
||||
#include <move_base2/core/navigation_request.h>
|
||||
#include <move_base2/core/state_machine.h>
|
||||
#include <move_base2/core/velocity_arbiter.h>
|
||||
#include <move_base2/ports/action_port.h>
|
||||
#include <move_base2/ports/clock_port.h>
|
||||
#include <move_base2/ports/controller_port.h>
|
||||
#include <move_base2/ports/mission_port.h>
|
||||
#include <move_base2/ports/planner_port.h>
|
||||
#include <move_base2/ports/pose_port.h>
|
||||
#include <move_base2/ports/recovery_port.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct ControlLoopDeps
|
||||
* @brief Các cổng mà control loop cần. Tất cả đều **non-owning** và bắt buộc khác null.
|
||||
*
|
||||
* @ref mission và @ref action được phép null: goal có thể đến thẳng từ contract host mà không qua
|
||||
* mission layer nào, và một hệ không có thiết bị thì không cần action. Yêu cầu MANG action sẽ bị
|
||||
* từ chối tại @ref ControlLoop::submit khi @ref action null — từ chối sớm thay vì kẹt giữa chừng.
|
||||
*/
|
||||
struct ControlLoopDeps
|
||||
{
|
||||
ClockPort* clock = nullptr;
|
||||
PosePort* pose = nullptr;
|
||||
PlannerPort* planner = nullptr;
|
||||
ControllerPort* controller = nullptr;
|
||||
RecoveryPort* recovery = nullptr;
|
||||
MissionPort* mission = nullptr; ///< Có thể null.
|
||||
ActionPort* action = nullptr; ///< Có thể null (D8) — null thì yêu cầu có action bị từ chối.
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct ProfileBinding
|
||||
* @brief Ánh xạ một kiểu chuyển động sang cặp planner và sai số mặc định.
|
||||
*
|
||||
* Bảng này là thứ thay thế sáu entry point gần như giống hệt nhau của contract host cũ: chúng chỉ
|
||||
* khác nhau ở đúng những trường dưới đây.
|
||||
*/
|
||||
struct ProfileBinding
|
||||
{
|
||||
std::string global_planner_name; ///< Alias plugin global planner.
|
||||
std::string local_planner_name; ///< Alias plugin local planner.
|
||||
double default_xy_tolerance = 0.15; ///< [m]
|
||||
double default_yaw_tolerance = 0.10; ///< [rad]
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct ControlLoopConfig
|
||||
* @brief Tham số của control loop.
|
||||
*/
|
||||
struct ControlLoopConfig
|
||||
{
|
||||
StateMachineConfig state_machine;
|
||||
VelocityLimits velocity;
|
||||
|
||||
/// [s] Chu kỳ danh nghĩa, chỉ dùng cho dt của cycle ĐẦU TIÊN khi chưa có mốc thời gian trước đó.
|
||||
double nominal_control_period = 0.05;
|
||||
|
||||
/// Frame gắn với thân robot. Lệnh vận tốc phát ra được đóng dấu bằng frame này — cmd_vel là vận
|
||||
/// tốc trong hệ thân xe, không phải hệ bản đồ hay odom.
|
||||
std::string robot_base_frame = "base_link";
|
||||
|
||||
/// Ánh xạ profile -> planner. Thiếu binding cho profile nào thì yêu cầu profile đó bị từ chối.
|
||||
ProfileBinding position;
|
||||
ProfileBinding docking;
|
||||
ProfileBinding go_straight;
|
||||
ProfileBinding rotate;
|
||||
|
||||
bool validate(std::string& error) const;
|
||||
std::string describe() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class ControlLoop
|
||||
* @brief Thân vòng điều khiển, chạy một cycle mỗi lần gọi @ref step.
|
||||
*
|
||||
* Trình tự trong một cycle, cố định và không được đảo:
|
||||
* 1. Đọc thời gian, đọc pose, tính dt thật và quãng đường chống quẩn.
|
||||
* 2. Chạy state machine với phản hồi thu được từ cycle TRƯỚC.
|
||||
* 3. Thi hành output: nhận yêu cầu, đẩy plan, khởi động/tick recovery, chạy controller.
|
||||
* 4. Đưa lệnh ứng viên qua bộ trọng tài để ra lệnh cuối cùng.
|
||||
* 5. Báo kết quả cho mission layer nếu state machine yêu cầu.
|
||||
*
|
||||
* Phản hồi trễ một cycle là có chủ đích và tường minh: nhờ vậy state machine luôn quyết định trước
|
||||
* rồi mới gọi cổng, không có vòng "gọi để biết nên gọi gì".
|
||||
*
|
||||
* Ở Phase 1 lớp này chạy đồng bộ, một thread, và lập plan ngay trong @ref step. Thread planner
|
||||
* riêng cùng bộ đệm plan ba lớp thuộc phần nối dây runtime, thêm sau; tách như vậy để phần quyết
|
||||
* định kiểm được mà không cần thread.
|
||||
*
|
||||
* @note Không thread-safe. Chỉ control thread được gọi — đây là thread duy nhất phát cmd_vel.
|
||||
*/
|
||||
class ControlLoop
|
||||
{
|
||||
public:
|
||||
ControlLoop() = default;
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình và các cổng.
|
||||
* @param[out] error Lý do không cấu hình được; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool configure(const ControlLoopConfig& config, const ControlLoopDeps& deps, std::string& error);
|
||||
|
||||
bool initialized() const
|
||||
{
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Nhận một yêu cầu mới.
|
||||
*
|
||||
* Yêu cầu được xếp vào chỗ chờ và chỉ thực sự bắt đầu ở cycle kế tiếp — đó là chỗ duy nhất
|
||||
* chuyển từ kIdle sang kPlanning, nên không có đường nào khác để một goal lọt vào lõi.
|
||||
*
|
||||
* @return false nếu chưa configure, goal không hợp lệ, hoặc không nạp được planner cho profile.
|
||||
*/
|
||||
bool submit(const NavigationRequest& request, std::string& reason);
|
||||
|
||||
void requestPause();
|
||||
void requestResume();
|
||||
void requestCancel();
|
||||
|
||||
/**
|
||||
* @brief Chạy một control cycle.
|
||||
* @return false khi state machine vừa bước vào state terminal ở cycle này.
|
||||
*/
|
||||
bool step();
|
||||
|
||||
/// @brief Lệnh vận tốc phát ra ở cycle gần nhất.
|
||||
const robot_geometry_msgs::Twist& lastCommand() const
|
||||
{
|
||||
return arbiter_.lastCommand();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Mốc thời gian của cycle gần nhất — cũng là thời điểm @ref lastCommand được sinh ra.
|
||||
*
|
||||
* Dùng để đóng dấu lệnh vận tốc gửi ra host. Host loại lệnh quá cũ theo dấu này, nên nó phải là
|
||||
* thời gian ĐỌC TỪ CỔNG ĐỒNG HỒ ở đầu cycle, không phải giờ hệ thống lúc host hỏi: nếu control
|
||||
* loop treo, dấu thời gian phải đứng yên để host nhìn thấy sự cố đó.
|
||||
*
|
||||
* @return Thời gian mặc định (0) khi chưa cycle nào chạy — luôn bị coi là quá hạn.
|
||||
*/
|
||||
robot::Time lastCycleTime() const
|
||||
{
|
||||
return last_cycle_time_;
|
||||
}
|
||||
|
||||
NavigationState state() const
|
||||
{
|
||||
return state_machine_.state();
|
||||
}
|
||||
|
||||
/// @brief Kết quả của yêu cầu vừa kết thúc; chuỗi rỗng nếu chưa có yêu cầu nào kết thúc.
|
||||
const char* lastOutcome() const;
|
||||
|
||||
bool hasOutcome() const
|
||||
{
|
||||
return has_outcome_;
|
||||
}
|
||||
|
||||
/// @brief Số lần đã báo kết quả — dùng để kiểm bất biến "đúng một lần cho mỗi yêu cầu".
|
||||
std::size_t outcomeReportCount() const
|
||||
{
|
||||
return outcome_report_count_;
|
||||
}
|
||||
|
||||
const StateMachine& stateMachine() const
|
||||
{
|
||||
return state_machine_;
|
||||
}
|
||||
|
||||
const VelocityArbiter& arbiter() const
|
||||
{
|
||||
return arbiter_;
|
||||
}
|
||||
|
||||
/// @brief Cổng pose đang dùng; nullptr nếu chưa configure. Non-owning, không được cache lại.
|
||||
PosePort* posePort() const
|
||||
{
|
||||
return deps_.pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cổng controller đang dùng; nullptr nếu chưa configure. Non-owning, không cache lại.
|
||||
*
|
||||
* @warning Chỉ được dùng từ **control thread**. Có mặt ở đây để `NavigationServer` đẩy trần vận
|
||||
* tốc và vận tốc đo được xuống trong `spinOnce()`, chứ không phải để host gọi thẳng.
|
||||
*/
|
||||
ControllerPort* controllerPort() const
|
||||
{
|
||||
return deps_.controller;
|
||||
}
|
||||
|
||||
/// @brief Lý do chuyển state gần nhất. Chỉ đổi khi state đổi, nên log được mà không spam.
|
||||
const char* lastReason() const
|
||||
{
|
||||
return last_reason_;
|
||||
}
|
||||
|
||||
/// @brief Đưa loop về trạng thái ban đầu, giữ nguyên cấu hình và các cổng.
|
||||
void reset();
|
||||
|
||||
private:
|
||||
/// @brief Binding cho một profile; nullptr nếu profile chưa được cấu hình.
|
||||
const ProfileBinding* bindingFor(MotionProfile profile) const;
|
||||
|
||||
/**
|
||||
* @brief Thu kết quả lập plan bất đồng bộ và quy nó thành @ref planner_feedback_.
|
||||
*
|
||||
* Gọi ở **đầu** cycle, trước khi dựng dữ liệu vào cho state machine: state machine tiêu thụ phản
|
||||
* hồi rồi mới quyết định, nên kết quả phải có mặt trước lúc đó.
|
||||
*/
|
||||
void collectPlannerResult();
|
||||
|
||||
/// @brief Gọi controller và lấy lệnh ứng viên.
|
||||
void runController(robot_geometry_msgs::Twist& candidate);
|
||||
|
||||
/// @brief Quaternion có chuẩn hoá được không — goal hỏng phải bị từ chối ngay tại cửa.
|
||||
static bool isQuaternionValid(const robot_geometry_msgs::PoseStamped& pose);
|
||||
|
||||
ControlLoopConfig config_;
|
||||
ControlLoopDeps deps_;
|
||||
bool initialized_ = false;
|
||||
|
||||
StateMachine state_machine_;
|
||||
VelocityArbiter arbiter_;
|
||||
|
||||
NavigationRequest pending_request_;
|
||||
bool has_pending_request_ = false;
|
||||
NavigationRequest active_request_;
|
||||
bool has_active_request_ = false;
|
||||
|
||||
bool pause_requested_ = false;
|
||||
bool resume_requested_ = false;
|
||||
bool cancel_requested_ = false;
|
||||
|
||||
/// Phản hồi thu được ở cycle trước, dùng làm dữ liệu vào cho cycle này.
|
||||
PlannerFeedback planner_feedback_ = PlannerFeedback::kIdle;
|
||||
ControllerFeedback controller_feedback_ = ControllerFeedback::kIdle;
|
||||
RecoveryFeedback recovery_feedback_ = RecoveryFeedback::kIdle;
|
||||
ActionFeedback action_feedback_ = ActionFeedback::kIdle;
|
||||
|
||||
std::vector<robot_geometry_msgs::PoseStamped> latest_plan_;
|
||||
bool planner_running_ = false;
|
||||
|
||||
/**
|
||||
* Nhãn của yêu cầu đang chạy, cấp cho từng lượt lập plan.
|
||||
*
|
||||
* Lập plan mất hàng trăm ms — lâu hơn tuổi thọ của goal sinh ra nó trong trường hợp bị huỷ hay
|
||||
* thay bằng yêu cầu khác. Kết quả về sau mang nhãn cũ sẽ bị vứt, thay vì được bám theo tới một
|
||||
* goal không còn ai yêu cầu.
|
||||
*/
|
||||
std::uint64_t plan_tag_ = 0;
|
||||
|
||||
robot::Time last_cycle_time_;
|
||||
bool has_last_cycle_time_ = false;
|
||||
|
||||
robot_geometry_msgs::PoseStamped oscillation_origin_;
|
||||
bool has_oscillation_origin_ = false;
|
||||
|
||||
bool has_outcome_ = false;
|
||||
NavigationOutcome last_outcome_ = NavigationOutcome::kFailed;
|
||||
std::size_t outcome_report_count_ = 0;
|
||||
|
||||
const char* last_reason_ = "";
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CONTROL_LOOP_H_
|
||||
114
include/move_base2/core/navigation_request.h
Normal file
114
include/move_base2/core/navigation_request.h
Normal file
@@ -0,0 +1,114 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — một yêu cầu navigation, gộp mọi kiểu chuyển động về một struct.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_
|
||||
#define MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_protocol_msgs/Action.h>
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @enum MotionProfile
|
||||
* @brief Kiểu chuyển động của một yêu cầu.
|
||||
*
|
||||
* Profile quyết định local planner nào được nạp và global planner nào được swap. Nó là thứ duy nhất
|
||||
* thực sự khác nhau giữa sáu entry point của contract host cũ (moveTo ×2, dockTo ×2, moveStraightTo,
|
||||
* rotateTo) — phần còn lại của sáu hàm đó giống hệt nhau.
|
||||
*/
|
||||
enum class MotionProfile
|
||||
{
|
||||
kPosition, ///< Di chuyển tới một pose bất kỳ.
|
||||
kDocking, ///< Ghép nối vào marker (sạc, trạm nạp…).
|
||||
kGoStraight, ///< Đi thẳng theo trục X của robot.
|
||||
kRotate ///< Xoay tại chỗ tới hướng đích.
|
||||
};
|
||||
|
||||
/// @brief Tên profile dạng chuỗi, cho log và config.
|
||||
const char* toString(MotionProfile profile);
|
||||
|
||||
/**
|
||||
* @struct GoalTolerance
|
||||
* @brief Sai số chấp nhận được tại đích.
|
||||
*
|
||||
* Quy ước: giá trị <= 0 nghĩa là "dùng default của profile trong config", không phải "yêu cầu sai số
|
||||
* bằng 0". Quy ước này kế thừa từ contract host cũ (tham số mặc định 0.0) nên không đổi được.
|
||||
*/
|
||||
struct GoalTolerance
|
||||
{
|
||||
double xy = 0.0; ///< [m]
|
||||
double yaw = 0.0; ///< [rad]
|
||||
|
||||
/// @brief Có ghi đè default của profile hay không.
|
||||
bool hasXy() const
|
||||
{
|
||||
return xy > 0.0;
|
||||
}
|
||||
|
||||
bool hasYaw() const
|
||||
{
|
||||
return yaw > 0.0;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct NavigationRequest
|
||||
* @brief Một chặng navigation cần chạy.
|
||||
*
|
||||
* Đây là kiểu dữ liệu duy nhất đi vào lõi. Mọi nguồn goal — contract host, action server, mission
|
||||
* layer — đều quy về struct này, nên lõi chỉ có một đường vào và một chỗ để khoá.
|
||||
*/
|
||||
struct NavigationRequest
|
||||
{
|
||||
MotionProfile profile = MotionProfile::kPosition;
|
||||
|
||||
/**
|
||||
* D8: false = yêu cầu chỉ-có-action, không có chặng navigation nào. Runtime bỏ qua
|
||||
* planning/controlling và vào thẳng thực thi action; @ref goal khi đó không có nghĩa và không
|
||||
* được validate. Yêu cầu không có goal lẫn action là vi phạm contract và bị từ chối tại cửa.
|
||||
*/
|
||||
bool has_goal = true;
|
||||
|
||||
/// Pose đích. Frame bất kỳ; phần nối dây chịu trách nhiệm đưa về global frame trước khi lập plan.
|
||||
robot_geometry_msgs::PoseStamped goal;
|
||||
|
||||
GoalTolerance tolerance;
|
||||
|
||||
/**
|
||||
* D8: action của mission, chạy SAU khi tới goal (hoặc ngay lập tức nếu @ref has_goal false),
|
||||
* đúng thứ tự trong vector. Mission layer chép nguyên từ mission output, runtime không diễn giải
|
||||
* nội dung — việc đó thuộc ActionHandler phía sau ActionPort.
|
||||
*/
|
||||
std::vector<robot_protocol_msgs::Action> actions;
|
||||
|
||||
/// Chỉ dùng cho @ref MotionProfile::kDocking; rỗng với các profile khác.
|
||||
std::string marker;
|
||||
|
||||
/// Order gốc nếu yêu cầu đến từ giao thức fleet; null nếu là goal trực tiếp.
|
||||
std::shared_ptr<robot_protocol_msgs::Order> order;
|
||||
|
||||
/**
|
||||
* Số hiệu chặng do mission layer cấp. 0 = goal trực tiếp, không thuộc mission nào.
|
||||
*
|
||||
* Giá trị này được echo nguyên vẹn khi báo kết quả, và là khoá để giữ bất biến "mỗi chặng chỉ
|
||||
* được báo kết quả đúng một lần".
|
||||
*/
|
||||
std::uint64_t mission_sequence_id = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CORE_NAVIGATION_REQUEST_H_
|
||||
61
include/move_base2/core/navigation_state.h
Normal file
61
include/move_base2/core/navigation_state.h
Normal file
@@ -0,0 +1,61 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — tập state của navigation runtime.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CORE_NAVIGATION_STATE_H_
|
||||
#define MOVE_BASE2_CORE_NAVIGATION_STATE_H_
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @enum NavigationState
|
||||
* @brief State của một yêu cầu navigation.
|
||||
*
|
||||
* Ba điểm khác bản runtime thế hệ 1, đều có lý do vận hành:
|
||||
*
|
||||
* 1. `kRecovering` thay cho `CLEARING` và là state **có thời lượng** (nhiều control cycle), không
|
||||
* phải một lời gọi blocking. Bắt buộc như vậy vì recovery thế hệ 2 trả kết quả từng tick và có
|
||||
* thể phát vận tốc — nghĩa là nó phải chạy cùng nhịp với controller.
|
||||
*
|
||||
* 2. `kPaused` và `kCancelling` là state thật, không phải cờ đọc rải rác trong vòng lặp. Nhờ vậy
|
||||
* đường huỷ chỉ còn một chỗ duy nhất thay vì lặp lại ở nhánh try và nhánh catch.
|
||||
*
|
||||
* 3. `kSucceeded` / `kAborted` / `kCancelled` là state, không phải `return` giữa hàm. Đây là thứ
|
||||
* giữ được bất biến "báo kết quả đúng một lần cho mỗi yêu cầu".
|
||||
*
|
||||
* 4. `kExecutingActions` (D8): runtime điều phối trọn một mission — tới goal xong còn chạy nốt
|
||||
* các action của mission (nâng/hạ, sạc, chờ…) rồi mới báo kết quả. Trong state này KHÔNG ai
|
||||
* được phát vận tốc: action cần chuyển động phải được mô hình hoá thành motion profile của
|
||||
* navigation, không phải action.
|
||||
*/
|
||||
enum class NavigationState
|
||||
{
|
||||
kIdle, ///< Không có yêu cầu nào đang chạy. Vận tốc = 0.
|
||||
kPlanning, ///< Đang chờ global planner ra plan. Vận tốc = 0.
|
||||
kControlling, ///< Đang bám plan. Local planner là nguồn vận tốc.
|
||||
kRecovering, ///< Đang chạy một recovery behavior. Recovery là nguồn vận tốc.
|
||||
kExecutingActions, ///< Đang chạy action của mission tại chỗ (D8). Vận tốc = 0.
|
||||
kPaused, ///< Tạm dừng theo yêu cầu. Vận tốc = 0.
|
||||
kCancelling, ///< Đang giảm tốc để huỷ. Vận tốc = 0, chờ robot dừng hẳn.
|
||||
kSucceeded, ///< Terminal: đạt goal và mọi action đã xong.
|
||||
kAborted, ///< Terminal: hết cách, không đạt được goal hoặc action thất bại.
|
||||
kCancelled ///< Terminal: đã huỷ theo yêu cầu và robot đã dừng.
|
||||
};
|
||||
|
||||
/// @brief Tên state dạng chuỗi, dùng cho log và cho phần assert chuỗi state trong test.
|
||||
const char* toString(NavigationState state);
|
||||
|
||||
/// @brief State kết thúc một yêu cầu (kSucceeded/kAborted/kCancelled).
|
||||
bool isTerminal(NavigationState state);
|
||||
|
||||
/// @brief State mà theo thiết kế KHÔNG được phép có vận tốc khác 0 — bất biến an toàn.
|
||||
bool mustBeStopped(NavigationState state);
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CORE_NAVIGATION_STATE_H_
|
||||
327
include/move_base2/core/state_machine.h
Normal file
327
include/move_base2/core/state_machine.h
Normal file
@@ -0,0 +1,327 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — state machine của navigation runtime. Logic thuần, không I/O.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CORE_STATE_MACHINE_H_
|
||||
#define MOVE_BASE2_CORE_STATE_MACHINE_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
#include <robot/time.h>
|
||||
|
||||
#include <move_base2/core/navigation_state.h>
|
||||
#include <move_base2/core/velocity_arbiter.h>
|
||||
#include <move_base2/ports/mission_port.h>
|
||||
#include <move_base2/ports/recovery_port.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/// @brief Tin từ global planner cho cycle này.
|
||||
enum class PlannerFeedback
|
||||
{
|
||||
kIdle, ///< Chưa có tin gì mới kể từ cycle trước.
|
||||
kBusy, ///< Đang lập plan, chưa có kết quả.
|
||||
kPlanReady, ///< Có plan mới, không rỗng, sẵn sàng đẩy xuống controller.
|
||||
kFailed ///< Lần lập plan gần nhất thất bại.
|
||||
};
|
||||
|
||||
/// @brief Tin từ local planner cho cycle này.
|
||||
enum class ControllerFeedback
|
||||
{
|
||||
kIdle, ///< Chưa chạy controller ở cycle này.
|
||||
kCommandValid, ///< Sinh được lệnh vận tốc hợp lệ.
|
||||
kNoValidCommand, ///< Không sinh được lệnh hợp lệ.
|
||||
kGoalReached ///< Đã tới đích trong sai số cho phép.
|
||||
};
|
||||
|
||||
/// @brief Tin từ recovery cho cycle này.
|
||||
enum class RecoveryFeedback
|
||||
{
|
||||
kIdle,
|
||||
kRunning,
|
||||
kSucceeded,
|
||||
kFailed
|
||||
};
|
||||
|
||||
/// @brief Tin từ action đang chạy cho cycle này (D8).
|
||||
enum class ActionFeedback
|
||||
{
|
||||
kIdle, ///< Chưa chạy action ở cycle này.
|
||||
kRunning,
|
||||
kSucceeded, ///< Action HIỆN TẠI xong; còn action kế tiếp hay không do state machine quyết.
|
||||
kFailed
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct StateMachineConfig
|
||||
* @brief Tham số điều khiển hành vi chuyển state.
|
||||
*
|
||||
* Theo quy ước của repo, mọi tham số đều có default trong struct, có @ref validate kiểm miền giá
|
||||
* trị, và có ghi đơn vị ngay tại khai báo.
|
||||
*/
|
||||
struct StateMachineConfig
|
||||
{
|
||||
/// [s] Thời gian tối đa được ở trạng thái chưa có plan hợp lệ trước khi vào recovery. <= 0 = tắt.
|
||||
double planner_patience = 5.0;
|
||||
|
||||
/// [s] Thời gian tối đa không sinh được lệnh hợp lệ trước khi vào recovery. <= 0 = tắt.
|
||||
double controller_patience = 15.0;
|
||||
|
||||
/// [s] Thời gian tối đa được quẩn trong bán kính @ref oscillation_distance. <= 0 = tắt.
|
||||
double oscillation_timeout = 0.0;
|
||||
|
||||
/**
|
||||
* [s] Trần thời gian cho MỘT action, tính từ lúc start. <= 0 = tắt (mặc định).
|
||||
*
|
||||
* Đây là lưới an toàn CUỐI CÙNG, không phải cơ chế timeout chính: mỗi ActionHandler phải tự
|
||||
* timeout theo hiểu biết thiết bị của nó ("nâng kệ quá 20 s là bất thường" vs "sạc 30 phút là
|
||||
* bình thường"), và mission layer còn `mission_timeout` cho cả chặng. Chỉ bật giá trị này khi
|
||||
* deployment biết chắc mọi action đều ngắn hơn một trần chung. Quá hạn -> cancel action +
|
||||
* ABORTED.
|
||||
*/
|
||||
double action_patience = 0.0;
|
||||
|
||||
/// [m] Đi được quá khoảng này thì coi như không còn quẩn, đồng hồ oscillation reset.
|
||||
double oscillation_distance = 0.5;
|
||||
|
||||
/// Số lần lập plan hỏng liên tiếp tối đa. < 0 = không giới hạn (chỉ chặn bằng planner_patience).
|
||||
int max_planning_retries = -1;
|
||||
|
||||
/// Số recovery behavior đã nạp được. 0 = không có đường phục hồi, lỗi là ABORTED ngay.
|
||||
std::size_t recovery_behavior_count = 0;
|
||||
|
||||
/// Cho phép chạy recovery hay không. false = mọi lỗi dẫn thẳng tới ABORTED.
|
||||
bool recovery_enabled = true;
|
||||
|
||||
/**
|
||||
* @brief Kiểm miền giá trị.
|
||||
* @param[out] error Mô tả tham số sai; chỉ được ghi khi hàm trả false.
|
||||
*/
|
||||
bool validate(std::string& error) const;
|
||||
|
||||
/// @brief Kết xuất cấu hình thành text nhiều dòng, để log một lần lúc khởi tạo.
|
||||
std::string describe() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct StateMachineInput
|
||||
* @brief Toàn bộ dữ liệu vào của một lần chuyển state.
|
||||
*
|
||||
* Cố ý không chứa costmap, tf, hay con trỏ tới bất kỳ thành phần nào: state machine phải kiểm được
|
||||
* bằng bảng, không cần dựng runtime.
|
||||
*/
|
||||
struct StateMachineInput
|
||||
{
|
||||
robot::Time now; ///< Thời điểm của cycle này.
|
||||
|
||||
bool has_pending_request = false; ///< Có yêu cầu mới đang chờ được nhận.
|
||||
|
||||
/// D8 — hình dạng của yêu cầu đang chờ; chỉ có nghĩa khi @ref has_pending_request. State machine
|
||||
/// chốt lại hai giá trị này tại cycle nhận yêu cầu, các cycle sau không đọc nữa.
|
||||
bool pending_request_has_goal = true;
|
||||
std::size_t pending_request_action_count = 0;
|
||||
|
||||
bool pause_requested = false;
|
||||
bool resume_requested = false;
|
||||
bool cancel_requested = false;
|
||||
|
||||
PlannerFeedback planner = PlannerFeedback::kIdle;
|
||||
ControllerFeedback controller = ControllerFeedback::kIdle;
|
||||
RecoveryFeedback recovery = RecoveryFeedback::kIdle;
|
||||
ActionFeedback action = ActionFeedback::kIdle;
|
||||
|
||||
/**
|
||||
* Lấy được pose robot hay không.
|
||||
*
|
||||
* false nghĩa là TF thiếu hoặc quá hạn. Khi đó state machine ép nguồn vận tốc về kNone ở MỌI
|
||||
* state: không biết robot ở đâu thì không được cho nó chạy, kể cả đang recovery. Các đồng hồ
|
||||
* kiên nhẫn vẫn chạy, nên mất TF kéo dài cuối cùng vẫn dẫn tới recovery rồi ABORTED thay vì treo.
|
||||
*/
|
||||
bool pose_available = true;
|
||||
|
||||
/// Robot đã dừng hẳn chưa (|v| <= ngưỡng). Dùng để rời kCancelling.
|
||||
bool robot_stopped = true;
|
||||
|
||||
/**
|
||||
* Họ output của behavior sẽ chạy / đang chạy ở cycle này.
|
||||
*
|
||||
* Bên gọi lấy từ `RecoveryPort::outputKind(nextRecoveryIndex())` — cùng một chỉ số dùng cho cả
|
||||
* cycle khởi động lẫn các cycle tick, vì chỉ số chỉ tăng khi behavior kết thúc.
|
||||
*
|
||||
* State machine dùng nó để KHÔNG trao quyền phát vận tốc cho behavior không lái robot (đợi, xoá
|
||||
* costmap). Chỉ có nghĩa khi state là kRecovering.
|
||||
*/
|
||||
RecoveryOutputKind active_recovery_output = RecoveryOutputKind::kVelocity;
|
||||
|
||||
/// [m] Quãng đường đi được kể từ mốc oscillation gần nhất.
|
||||
double travelled_since_oscillation_reset = 0.0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct StateMachineOutput
|
||||
* @brief Việc cần làm sau một lần chuyển state.
|
||||
*
|
||||
* Đây là danh sách hành động, không phải lời gọi: state machine không tự thi hành gì. Nhờ vậy test
|
||||
* kiểm được "đáng lẽ phải làm gì" tách rời khỏi "làm thế nào".
|
||||
*/
|
||||
struct StateMachineOutput
|
||||
{
|
||||
NavigationState state = NavigationState::kIdle;
|
||||
bool state_changed = false;
|
||||
|
||||
bool accept_request = false; ///< Lấy yêu cầu đang chờ ra khỏi hàng đợi và coi là yêu cầu hiện tại.
|
||||
bool start_planner = false; ///< Yêu cầu planner bắt đầu/tiếp tục lập plan.
|
||||
bool stop_planner = false; ///< Yêu cầu planner ngừng.
|
||||
bool apply_plan = false; ///< Đẩy plan mới nhất xuống controller.
|
||||
bool run_controller = false; ///< Gọi controller ở cycle này.
|
||||
|
||||
bool start_recovery = false; ///< Khởi động behavior @ref recovery_index với @ref recovery_trigger.
|
||||
bool tick_recovery = false; ///< Gọi update() của behavior đang chạy.
|
||||
bool cancel_recovery = false; ///< Yêu cầu behavior đang chạy dừng.
|
||||
|
||||
bool start_action = false; ///< D8: khởi động action thứ @ref action_index của yêu cầu hiện tại.
|
||||
bool tick_action = false; ///< D8: gọi update() của action đang chạy.
|
||||
bool cancel_action = false; ///< D8: yêu cầu action đang chạy dừng an toàn.
|
||||
|
||||
/// Chỉ có nghĩa khi @ref start_action — action nào trong danh sách của yêu cầu cần khởi động.
|
||||
std::size_t action_index = 0;
|
||||
|
||||
bool reset_oscillation_origin = false; ///< Đặt lại mốc đo quãng đường chống quẩn về pose hiện tại.
|
||||
|
||||
bool report_outcome = false; ///< Báo kết quả chặng — đúng một lần.
|
||||
NavigationOutcome outcome = NavigationOutcome::kFailed;
|
||||
|
||||
/// Chỉ có nghĩa khi @ref start_recovery — behavior nào cần khởi động và vì lý do gì.
|
||||
std::size_t recovery_index = 0;
|
||||
RecoveryTrigger recovery_trigger = RecoveryTrigger::kPlanningFailed;
|
||||
|
||||
/// Nguồn được phép phát vận tốc ở cycle này. Luôn có đúng một nguồn.
|
||||
VelocitySource velocity_source = VelocitySource::kNone;
|
||||
|
||||
/// Lý do chuyển state, chuỗi hằng. Chỉ log khi @ref state_changed để không spam control loop.
|
||||
const char* reason = "";
|
||||
};
|
||||
|
||||
/**
|
||||
* @class StateMachine
|
||||
* @brief Bộ chuyển state của navigation runtime.
|
||||
*
|
||||
* Logic thuần: không đụng costmap, không đụng tf, không log, không cấp phát trong @ref update.
|
||||
* Toàn bộ bảng chuyển được mô tả trong `docs/STATE_MACHINE.md` và bảng đó là nguồn chuẩn.
|
||||
*
|
||||
* Bất biến được giữ:
|
||||
* - Mỗi cycle có đúng MỘT nguồn vận tốc (@ref StateMachineOutput::velocity_source).
|
||||
* - @ref StateMachineOutput::run_controller, @ref StateMachineOutput::tick_recovery và
|
||||
* @ref StateMachineOutput::tick_action đôi một không bao giờ cùng true.
|
||||
* - @ref StateMachineOutput::report_outcome bật đúng một lần cho mỗi yêu cầu, tại cycle bước vào
|
||||
* state terminal — kể cả khi yêu cầu có action: kết quả chỉ được báo sau action cuối (D8).
|
||||
* - Ở state phải dừng (@ref mustBeStopped) thì velocity_source luôn là kNone; kExecutingActions
|
||||
* thuộc nhóm này — action không bao giờ đi kèm vận tốc.
|
||||
*
|
||||
* @note Không thread-safe. Chỉ control thread được gọi.
|
||||
*/
|
||||
class StateMachine
|
||||
{
|
||||
public:
|
||||
StateMachine() = default;
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình và đưa state machine về kIdle.
|
||||
* @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false.
|
||||
* @return false nếu cấu hình không hợp lệ — bên gọi KHÔNG được chạy tiếp với cấu hình hỏng.
|
||||
*/
|
||||
bool configure(const StateMachineConfig& config, std::string& error);
|
||||
|
||||
/// @brief Đã configure thành công hay chưa. @ref update trả kIdle nếu chưa.
|
||||
bool initialized() const
|
||||
{
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
/// @brief Chạy một cycle.
|
||||
StateMachineOutput update(const StateMachineInput& input);
|
||||
|
||||
/// @brief Đưa về kIdle, xoá mọi bộ đếm và đồng hồ. Dùng khi khởi động lại runtime.
|
||||
void reset();
|
||||
|
||||
NavigationState state() const
|
||||
{
|
||||
return state_;
|
||||
}
|
||||
|
||||
const StateMachineConfig& config() const
|
||||
{
|
||||
return config_;
|
||||
}
|
||||
|
||||
/// @brief Chỉ số behavior sẽ chạy ở lần vào recovery kế tiếp. Dùng để assert trong test.
|
||||
std::size_t nextRecoveryIndex() const
|
||||
{
|
||||
return recovery_index_;
|
||||
}
|
||||
|
||||
/// @brief Chỉ số action đang chạy / sẽ chạy kế tiếp của yêu cầu hiện tại. Dùng để assert trong test.
|
||||
std::size_t currentActionIndex() const
|
||||
{
|
||||
return action_index_;
|
||||
}
|
||||
|
||||
/// @brief Số lần lập plan hỏng liên tiếp tính từ lần vào kPlanning gần nhất.
|
||||
int planningRetries() const
|
||||
{
|
||||
return planning_retries_;
|
||||
}
|
||||
|
||||
/// @brief Thời gian [s] đã ở trong state hiện tại, tính tới @p now.
|
||||
double secondsInState(const robot::Time& now) const;
|
||||
|
||||
private:
|
||||
/// @brief Chuyển sang @p next và ghi nhận thời điểm vào state.
|
||||
void enter(NavigationState next, const robot::Time& now, const char* reason,
|
||||
StateMachineOutput& out);
|
||||
|
||||
/// @brief Bắt đầu một chu kỳ lập plan: reset đồng hồ kiên nhẫn và bộ đếm retry.
|
||||
void beginPlanningCycle(const robot::Time& now);
|
||||
|
||||
/**
|
||||
* @brief Vào recovery nếu còn behavior, ngược lại kết thúc bằng kAborted.
|
||||
* @param trigger Lý do vào recovery.
|
||||
*/
|
||||
void escalateToRecovery(RecoveryTrigger trigger, const robot::Time& now, const char* reason,
|
||||
StateMachineOutput& out);
|
||||
|
||||
/// @brief Vào state terminal và bật cờ báo kết quả đúng một lần.
|
||||
void finish(NavigationState terminal, NavigationOutcome outcome, const robot::Time& now,
|
||||
const char* reason, StateMachineOutput& out);
|
||||
|
||||
StateMachineConfig config_;
|
||||
bool initialized_ = false;
|
||||
|
||||
NavigationState state_ = NavigationState::kIdle;
|
||||
/// State để quay về sau kPaused. Không bao giờ là kPaused hay state terminal.
|
||||
NavigationState state_before_pause_ = NavigationState::kIdle;
|
||||
|
||||
robot::Time state_entered_at_;
|
||||
robot::Time last_valid_plan_; ///< Mốc đo planner_patience.
|
||||
robot::Time last_valid_control_; ///< Mốc đo controller_patience.
|
||||
robot::Time last_oscillation_reset_;
|
||||
|
||||
std::size_t recovery_index_ = 0;
|
||||
int planning_retries_ = 0;
|
||||
|
||||
/// D8 — hình dạng của yêu cầu hiện tại, chốt tại cycle nhận yêu cầu từ StateMachineInput.
|
||||
bool request_has_goal_ = true;
|
||||
std::size_t action_count_ = 0;
|
||||
std::size_t action_index_ = 0;
|
||||
robot::Time action_started_at_; ///< Mốc đo action_patience; đặt lại mỗi lần start action và khi resume.
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CORE_STATE_MACHINE_H_
|
||||
180
include/move_base2/core/velocity_arbiter.h
Normal file
180
include/move_base2/core/velocity_arbiter.h
Normal file
@@ -0,0 +1,180 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — bộ trọng tài vận tốc: ai được phát lệnh, và lệnh đó có an toàn không.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_CORE_VELOCITY_ARBITER_H_
|
||||
#define MOVE_BASE2_CORE_VELOCITY_ARBITER_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @enum VelocitySource
|
||||
* @brief Nguồn vận tốc hợp lệ tại một thời điểm. LUÔN chỉ có một.
|
||||
*/
|
||||
enum class VelocitySource
|
||||
{
|
||||
kNone, ///< Không ai được phát — lệnh ra ngoài là 0.
|
||||
kController, ///< Local planner.
|
||||
kRecovery ///< Recovery behavior đang chạy.
|
||||
};
|
||||
|
||||
const char* toString(VelocitySource source);
|
||||
|
||||
/**
|
||||
* @struct VelocityLimits
|
||||
* @brief Giới hạn động học áp cho mọi lệnh trước khi ra khỏi runtime.
|
||||
*
|
||||
* Đây là hàng rào cuối cùng, không phải bộ điều khiển: local planner đã có giới hạn riêng, nhưng
|
||||
* runtime không được tin bất kỳ nguồn nào — kể cả recovery behavior nạp từ plugin ngoài.
|
||||
*/
|
||||
struct VelocityLimits
|
||||
{
|
||||
double max_vel_x = 0.5; ///< [m/s] trần tốc độ tiến (dương).
|
||||
double min_vel_x = -0.2; ///< [m/s] trần tốc độ lùi (ÂM). 0 = cấm lùi.
|
||||
double max_vel_theta = 1.0; ///< [rad/s] trần tốc độ quay tuyệt đối.
|
||||
double max_accel_x = 1.0; ///< [m/s^2] trần biến thiên tốc độ dài, áp cho cả tăng và giảm.
|
||||
double max_accel_theta = 2.0; ///< [rad/s^2] trần biến thiên tốc độ quay.
|
||||
|
||||
/// [m/s] và [rad/s] — dưới ngưỡng này coi như đã dừng.
|
||||
double zero_velocity_epsilon = 1e-3;
|
||||
|
||||
/**
|
||||
* @brief Kiểm miền giá trị.
|
||||
* @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool validate(std::string& error) const;
|
||||
|
||||
/// @brief Kết xuất cấu hình thành text nhiều dòng, để log một lần lúc khởi tạo.
|
||||
std::string describe() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class VelocityArbiter
|
||||
* @brief Chốt chặn duy nhất giữa các nguồn lệnh và cmd_vel ra ngoài.
|
||||
*
|
||||
* Ba quy tắc, đều được viết thành assert + test chứ không chỉ là comment:
|
||||
*
|
||||
* 1. **Nguồn kNone phát lệnh 0 tức thì**, không giảm tốc dần. Lệnh vận tốc bị chốt lại ở tầng
|
||||
* dưới, nên nếu control loop dừng giữa lúc đang giảm tốc thì lệnh khác 0 cuối cùng vẫn còn hiệu
|
||||
* lực và robot chạy tiếp. Việc giảm tốc theo động học thuộc về bộ điều khiển bánh xe.
|
||||
*
|
||||
* 2. **Mọi lệnh phải qua sanitize.** NaN/Inf bị chặn thành 0 và được đếm lại; giá trị vượt trần bị
|
||||
* clamp theo @ref VelocityLimits; biến thiên bị clamp theo gia tốc và dt THẬT của cycle, không
|
||||
* phải chu kỳ danh nghĩa trong config.
|
||||
*
|
||||
* 3. **Đổi nguồn bắt buộc chèn ít nhất một cycle vận tốc 0.** Controller và recovery giữ trạng
|
||||
* thái gia tốc riêng; chuyển thẳng từ nguồn này sang nguồn kia gây giật. Ở thế hệ 1 vấn đề này
|
||||
* không tồn tại vì recovery chạy blocking và không phát vận tốc; ở thế hệ 2 thì có.
|
||||
*
|
||||
* @note Không thread-safe. Chỉ control thread — thread duy nhất được phát cmd_vel — gọi lớp này.
|
||||
*/
|
||||
class VelocityArbiter
|
||||
{
|
||||
public:
|
||||
VelocityArbiter() = default;
|
||||
|
||||
/**
|
||||
* @brief Nạp giới hạn và đưa bộ trọng tài về trạng thái dừng.
|
||||
* @param[out] error Mô tả tham số sai; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool configure(const VelocityLimits& limits, std::string& error);
|
||||
|
||||
bool initialized() const
|
||||
{
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Quyết định lệnh thực sự phát ra ở cycle này.
|
||||
* @param source Nguồn được state machine cho phép.
|
||||
* @param candidate Lệnh nguồn đó đề nghị. Bỏ qua khi @p source là kNone.
|
||||
* @param dt [s] Khoảng thời gian THẬT từ cycle trước. <= 0 thì bỏ qua giới hạn gia tốc.
|
||||
* @return Lệnh an toàn để gửi ra ngoài.
|
||||
*/
|
||||
robot_geometry_msgs::Twist arbitrate(VelocitySource source,
|
||||
const robot_geometry_msgs::Twist& candidate, double dt);
|
||||
|
||||
/// @brief Ép về 0 ngay lập tức, bỏ qua giới hạn gia tốc. Dùng cho dừng khẩn.
|
||||
robot_geometry_msgs::Twist emergencyStop();
|
||||
|
||||
/// @brief Lệnh đã phát ở cycle gần nhất.
|
||||
const robot_geometry_msgs::Twist& lastCommand() const
|
||||
{
|
||||
return last_command_;
|
||||
}
|
||||
|
||||
/// @brief Nguồn đang được công nhận sau lần @ref arbitrate gần nhất.
|
||||
VelocitySource activeSource() const
|
||||
{
|
||||
return active_source_;
|
||||
}
|
||||
|
||||
/// @brief Lệnh gần nhất có coi như đã dừng hay không (theo zero_velocity_epsilon).
|
||||
bool stopped() const;
|
||||
|
||||
/// @brief Số lần chặn được NaN/Inf. Khác 0 nghĩa là có nguồn đang trả dữ liệu hỏng.
|
||||
std::size_t nonFiniteRejections() const
|
||||
{
|
||||
return non_finite_rejections_;
|
||||
}
|
||||
|
||||
/// @brief Số lần phải clamp vì vượt trần vận tốc.
|
||||
std::size_t velocityClamps() const
|
||||
{
|
||||
return velocity_clamps_;
|
||||
}
|
||||
|
||||
/// @brief Số lần phải clamp vì vượt trần gia tốc.
|
||||
std::size_t accelerationClamps() const
|
||||
{
|
||||
return acceleration_clamps_;
|
||||
}
|
||||
|
||||
/// @brief Số cycle 0 đã chèn vào khi đổi nguồn.
|
||||
std::size_t handoverCycles() const
|
||||
{
|
||||
return handover_cycles_;
|
||||
}
|
||||
|
||||
/// @brief Về trạng thái dừng, xoá mọi bộ đếm và lịch sử lệnh.
|
||||
void reset();
|
||||
|
||||
const VelocityLimits& limits() const
|
||||
{
|
||||
return limits_;
|
||||
}
|
||||
|
||||
private:
|
||||
/// @brief Chặn NaN/Inf và clamp theo trần vận tốc. Trả về lệnh đã làm sạch.
|
||||
robot_geometry_msgs::Twist sanitize(const robot_geometry_msgs::Twist& candidate);
|
||||
|
||||
/// @brief Clamp biến thiên so với lệnh trước theo gia tốc và @p dt thật.
|
||||
robot_geometry_msgs::Twist limitAcceleration(const robot_geometry_msgs::Twist& target, double dt);
|
||||
|
||||
static robot_geometry_msgs::Twist zeroTwist();
|
||||
|
||||
VelocityLimits limits_;
|
||||
bool initialized_ = false;
|
||||
|
||||
VelocitySource active_source_ = VelocitySource::kNone;
|
||||
robot_geometry_msgs::Twist last_command_;
|
||||
|
||||
std::size_t non_finite_rejections_ = 0;
|
||||
std::size_t velocity_clamps_ = 0;
|
||||
std::size_t acceleration_clamps_ = 0;
|
||||
std::size_t handover_cycles_ = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_CORE_VELOCITY_ARBITER_H_
|
||||
206
include/move_base2/io/sensor_gateway.h
Normal file
206
include/move_base2/io/sensor_gateway.h
Normal file
@@ -0,0 +1,206 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cửa vào dữ liệu cảm biến: từ contract host tới các layer costmap.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_IO_SENSOR_GATEWAY_H_
|
||||
#define MOVE_BASE2_IO_SENSOR_GATEWAY_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_sensor_msgs/DepthCameraData.h>
|
||||
#include <robot_sensor_msgs/LaserScan.h>
|
||||
#include <robot_sensor_msgs/PointCloud.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
|
||||
// Chỉ cần con trỏ và một thành viên pimpl: định nghĩa thật nằm trong .cpp, nên file này không kéo
|
||||
// robot_costmap_2d và laser_filter vào mọi translation unit chạm tới nó.
|
||||
namespace robot_costmap_2d
|
||||
{
|
||||
class LayeredCostmap;
|
||||
}
|
||||
namespace laser_filter
|
||||
{
|
||||
class LaserScanSOR;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct SensorGatewayConfig
|
||||
* @brief Tham số của đường vào cảm biến.
|
||||
*/
|
||||
struct SensorGatewayConfig
|
||||
{
|
||||
/**
|
||||
* @brief Bật lọc statistical-outlier-removal cho LaserScan trước khi đưa vào costmap.
|
||||
*
|
||||
* Default **tắt**, và đó là quyết định có chủ đích. Bản `move_base` cũ chỉ lọc ở nhánh biên dịch
|
||||
* không-ROS (`#ifndef BUILD_WITH_ROS`), nên cùng một contract mà hai host nhìn thấy hai tập vật
|
||||
* cản khác nhau. Ở đây hành vi được hợp nhất thành một khoá YAML duy nhất; tắt theo mặc định giữ
|
||||
* đúng thứ robot đang chạy dưới host ROS hôm nay. Bật một bộ lọc theo mặc định là âm thầm xoá bớt
|
||||
* điểm vật cản — không phải thứ nên tự xảy ra.
|
||||
*/
|
||||
bool laser_sor_enabled = false;
|
||||
|
||||
/// [điểm] Số láng giềng gần nhất dùng để ước lượng khoảng cách trung bình. Chỉ dùng khi bật lọc.
|
||||
int laser_sor_mean_k = 10;
|
||||
|
||||
/// [-] Ngưỡng outlier = mean + hệ_số * stddev. Chỉ dùng khi bật lọc.
|
||||
double laser_sor_stddev_mul = 1.0;
|
||||
|
||||
/**
|
||||
* @brief Kiểm miền giá trị.
|
||||
* @param[out] error Mô tả tham số sai đầu tiên; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool validate(std::string& error) const;
|
||||
|
||||
/// @brief Kết xuất nhiều dòng để log đúng một lần lúc khởi tạo.
|
||||
std::string describe() const;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct SensorGatewayStats
|
||||
* @brief Đếm số phận của các mẫu đã đi qua cổng.
|
||||
*
|
||||
* Bản cũ bỏ mẫu **im lặng** ở ba chỗ khác nhau (chưa có costmap, layer tắt, sai tên topic) nên
|
||||
* "costmap không thấy vật cản" không có cách nào chẩn đoán ngoài việc đọc lại code. Các bộ đếm này
|
||||
* tồn tại để câu hỏi đó trả lời được bằng số.
|
||||
*/
|
||||
struct SensorGatewayStats
|
||||
{
|
||||
/// Số lần một layer thực sự nhận được dữ liệu (đếm theo cặp mẫu-layer).
|
||||
std::size_t delivered = 0;
|
||||
|
||||
/// Số mẫu bị bỏ vì chưa có costmap nào được gắn.
|
||||
std::size_t dropped_no_costmap = 0;
|
||||
|
||||
/// Số lần bỏ qua một layer vì layer đó đang tắt (`enabled: false`).
|
||||
std::size_t skipped_disabled = 0;
|
||||
|
||||
/// Số lần `handleImpl` của một layer ném exception.
|
||||
std::size_t layer_exceptions = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class SensorGateway
|
||||
* @brief Đưa dữ liệu cảm biến từ contract host vào đúng các layer của hai costmap.
|
||||
*
|
||||
* Đây là file **duy nhất** trong `move_base2` biết tới `robot_costmap_2d`. Lõi quyết định
|
||||
* (`StateMachine`, `ControlLoop`) không nhìn thấy costmap, và đó là thứ giữ cho lõi kiểm được bằng
|
||||
* bảng thay vì phải dựng một costmap thật.
|
||||
*
|
||||
* ## Ba contract ẩn phải giữ đúng
|
||||
*
|
||||
* 1. **Kiểu phải khớp chính xác.** `Layer::dataCallBack<T>` xoá kiểu về `void*` + `std::type_info`;
|
||||
* layer so sánh bằng `typeid`. Sai kiểu không gây lỗi biên dịch mà rơi im lặng — vì vậy cổng này
|
||||
* phơi ra các hàm push có kiểu cụ thể chứ không phải một template mở.
|
||||
* Riêng depth camera phải truyền đúng dạng `ConstPtr`, không phải giá trị.
|
||||
* 2. **`name` là khoá topic, không phải nhãn.** Layer tự lọc lần hai: `StaticLayer` so với
|
||||
* `map_topic`, `ObstacleLayer` so với `topic` của từng observation source trong YAML. Tên sai
|
||||
* một chữ là mất hẳn một cảm biến, không có cảnh báo nào.
|
||||
* 3. **Thứ tự và quyền sở hữu.** Con trỏ `LayeredCostmap` là **non-owning**; chủ sở hữu là
|
||||
* `NavigationServer`. Cổng này không cache `Costmap2D*` bên trong.
|
||||
*
|
||||
* ## Khác biệt có chủ đích so với `move_base` cũ
|
||||
*
|
||||
* - **Lọc layer chỉ theo `getType()`.** Bản cũ dùng `getType() == type || getName() == name`. Vế
|
||||
* tên là code chết trong thực tế (tên layer là `obstacles`/`inflation`, tên sensor là
|
||||
* `/b_scan`/`/map` — hai tập không giao nhau) nhưng lại là một cái bẫy: đặt tên một layer trùng
|
||||
* tên topic sẽ đẩy dữ liệu vào layer sai, và `InflationLayer::handleImpl` chỉ biết log error nên
|
||||
* sẽ spam ở đúng tần số cảm biến.
|
||||
* - **Bỏ qua layer đang tắt.** `ObstacleLayer::handleImpl` tự thoát ngay khi `enabled_` false, nên
|
||||
* bản cũ làm đủ việc rồi vứt. Ở đây bỏ sớm và **đếm lại**.
|
||||
* - **try/catch quanh từng layer**, không phải quanh cả vòng lặp: một layer ném exception không
|
||||
* được làm các layer sau đó mất luôn mẫu dữ liệu đó.
|
||||
*
|
||||
* @note Không thread-safe. Người gọi (`NavigationServer`) chịu trách nhiệm tuần tự hoá, và **không
|
||||
* được giữ lock dữ liệu của mình trong lúc gọi**: `StaticLayer::incomingMap` có thể gọi
|
||||
* `LayeredCostmap::resizeMap`, hàm này chờ mutex master costmap và có thể đứng trọn một chu
|
||||
* kỳ `updateMap`.
|
||||
*/
|
||||
class SensorGateway
|
||||
{
|
||||
public:
|
||||
SensorGateway();
|
||||
~SensorGateway();
|
||||
|
||||
SensorGateway(const SensorGateway&) = delete;
|
||||
SensorGateway& operator=(const SensorGateway&) = delete;
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình.
|
||||
* @param[out] error Lý do không cấu hình được; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool configure(const SensorGatewayConfig& config, std::string& error);
|
||||
|
||||
/**
|
||||
* @brief Gắn hai costmap đích. Cả hai đều **non-owning** và đều được phép null.
|
||||
*
|
||||
* Null nghĩa là chưa dựng costmap đó — mẫu tới sẽ bị bỏ nhưng được **đếm** và log một lần, thay
|
||||
* cho kiểu `if (!costmap) return;` im lặng của bản cũ.
|
||||
*
|
||||
* Gọi lại hàm này ghi đè con trỏ cũ; dùng khi costmap bị dựng lại.
|
||||
*/
|
||||
void attach(robot_costmap_2d::LayeredCostmap* global, robot_costmap_2d::LayeredCostmap* local);
|
||||
|
||||
/// @brief Đã có ít nhất một costmap đích hay chưa.
|
||||
bool attached() const;
|
||||
|
||||
/**
|
||||
* @brief Áp bộ lọc đã cấu hình lên một mẫu laser.
|
||||
*
|
||||
* Tách khỏi @ref pushLaserScan để người gọi **cất đúng bản đã lọc** — bản cũ cũng lưu bản đã lọc,
|
||||
* và nếu getter của contract host trả bản thô trong khi costmap thấy bản lọc thì hai nguồn sự
|
||||
* thật sẽ lệch nhau.
|
||||
*
|
||||
* @return Chính @p scan khi lọc đang tắt.
|
||||
*/
|
||||
robot_sensor_msgs::LaserScan prepareLaserScan(const robot_sensor_msgs::LaserScan& scan) const;
|
||||
|
||||
// ================================================================================================
|
||||
// Đẩy vào costmap. Tên tham số `name` chính là khoá topic mà layer sẽ so — xem contract ẩn #2.
|
||||
// ================================================================================================
|
||||
|
||||
void pushStaticMap(const std::string& name, const robot_nav_msgs::OccupancyGrid& map);
|
||||
void pushLaserScan(const std::string& name, const robot_sensor_msgs::LaserScan& scan);
|
||||
void pushPointCloud(const std::string& name, const robot_sensor_msgs::PointCloud& cloud);
|
||||
void pushPointCloud2(const std::string& name, const robot_sensor_msgs::PointCloud2& cloud);
|
||||
void pushDepthCameraData(const std::string& topic,
|
||||
const robot_sensor_msgs::DepthCameraData::ConstPtr& data);
|
||||
|
||||
const SensorGatewayStats& stats() const
|
||||
{
|
||||
return stats_;
|
||||
}
|
||||
|
||||
void resetStats()
|
||||
{
|
||||
stats_ = SensorGatewayStats();
|
||||
}
|
||||
|
||||
private:
|
||||
/// @brief Cảnh báo lúc gắn nếu costmap có layer kiểu ObstacleLayer thuần — chúng sẽ không nhận gì.
|
||||
void warnAboutUnreachableLayers(robot_costmap_2d::LayeredCostmap* costmap, const char* which) const;
|
||||
|
||||
SensorGatewayConfig config_;
|
||||
bool configured_ = false;
|
||||
|
||||
robot_costmap_2d::LayeredCostmap* global_costmap_ = nullptr;
|
||||
robot_costmap_2d::LayeredCostmap* local_costmap_ = nullptr;
|
||||
|
||||
std::unique_ptr<laser_filter::LaserScanSOR> laser_sor_;
|
||||
|
||||
SensorGatewayStats stats_;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_IO_SENSOR_GATEWAY_H_
|
||||
241
include/move_base2/navigation_server.h
Normal file
241
include/move_base2/navigation_server.h
Normal file
@@ -0,0 +1,241 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — facade hiện thực contract host BaseNavigation.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_NAVIGATION_SERVER_H_
|
||||
#define MOVE_BASE2_NAVIGATION_SERVER_H_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <move_base_core/navigation.h>
|
||||
|
||||
#include <move_base2/control_loop.h>
|
||||
#include <move_base2/core/navigation_request.h>
|
||||
#include <move_base2/io/sensor_gateway.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class NavigationServer
|
||||
* @brief Lớp biên duy nhất giữa host và lõi navigation.
|
||||
*
|
||||
* Nhiệm vụ đúng ba việc, không hơn:
|
||||
* 1. Quy sáu entry point di chuyển của contract host về một @ref NavigationRequest duy nhất.
|
||||
* 2. Nhận dữ liệu sensor từ host: cất giữ, rồi chuyển tiếp cho @ref SensorGateway.
|
||||
* 3. Kết xuất trạng thái lõi ra đúng kiểu mà host mong đợi.
|
||||
*
|
||||
* Contract `robot::move_base_core::BaseNavigation` được giữ **nguyên vẹn từng chữ ký**: ba host
|
||||
* đang phụ thuộc vào nó (bộ điều khiển phía ROS, C API cho .NET, và bản chạy độc lập). Đổi contract
|
||||
* đồng nghĩa với sửa cả phần binding sang ngôn ngữ khác.
|
||||
*
|
||||
* @note Phần nối dây tới planner/controller/recovery thật chưa có, nên @ref initialize chưa dựng
|
||||
* được các cổng runtime. Test và phần nối dây tự bơm cổng vào qua @ref configureLoop, và bơm
|
||||
* costmap vào qua @ref attachCostmaps. Chưa gắn costmap thì dữ liệu sensor vẫn được cất giữ
|
||||
* nhưng không tới được layer nào — @ref SensorGatewayStats::dropped_no_costmap đếm lại số đó.
|
||||
*/
|
||||
class NavigationServer : public robot::move_base_core::BaseNavigation
|
||||
{
|
||||
public:
|
||||
NavigationServer();
|
||||
~NavigationServer() override;
|
||||
|
||||
// ==============================================================================================
|
||||
// Cấu hình lõi — không thuộc contract host, dùng cho phần nối dây và cho test.
|
||||
// ==============================================================================================
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình và các cổng cho control loop.
|
||||
* @param[out] error Lý do không cấu hình được.
|
||||
*/
|
||||
bool configureLoop(const ControlLoopConfig& config, const ControlLoopDeps& deps,
|
||||
std::string& error);
|
||||
|
||||
/// @brief Chạy một control cycle. @return false khi yêu cầu hiện tại vừa kết thúc.
|
||||
bool spinOnce();
|
||||
|
||||
const ControlLoop& loop() const
|
||||
{
|
||||
return loop_;
|
||||
}
|
||||
|
||||
ControlLoop& loop()
|
||||
{
|
||||
return loop_;
|
||||
}
|
||||
|
||||
/// @brief Lý do từ chối gần nhất của một lời gọi di chuyển; chuỗi rỗng nếu chưa từ chối lần nào.
|
||||
const std::string& lastRejectReason() const
|
||||
{
|
||||
return last_reject_reason_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình cho đường vào cảm biến.
|
||||
*
|
||||
* Không bắt buộc: chưa gọi thì cổng chạy với default (lọc laser tắt).
|
||||
*
|
||||
* @param[out] error Lý do không cấu hình được.
|
||||
*/
|
||||
bool configureSensors(const SensorGatewayConfig& config, std::string& error);
|
||||
|
||||
/**
|
||||
* @brief Gắn hai costmap đích cho dữ liệu cảm biến, rồi phát lại các static map đã nhận.
|
||||
*
|
||||
* Phát lại là bắt buộc, không phải tiện ích: dữ liệu tới **trước** khi costmap tồn tại sẽ bị bỏ,
|
||||
* và trong thực tế `/map` gần như luôn tới trước — bản `move_base` cũ phải bù bằng cặp biến public
|
||||
* `map_save_`/`map_name_save_` mà host tự gán. Ở đây nguồn phát lại là chính @c static_maps_ đã
|
||||
* cất, cộng thêm `map_save_` nếu host có dùng đường cũ đó.
|
||||
*
|
||||
* Cả hai con trỏ **non-owning** và được phép null. Gọi lại được khi costmap bị dựng lại.
|
||||
*
|
||||
* @warning Phải gọi từ thread host và **không** song song với các hàm `add*` — @ref SensorGateway
|
||||
* không thread-safe. Trong thực tế đây là bước khởi tạo, chạy trước khi sensor bắt đầu.
|
||||
*/
|
||||
void attachCostmaps(robot_costmap_2d::LayeredCostmap* global,
|
||||
robot_costmap_2d::LayeredCostmap* local);
|
||||
|
||||
/// @brief Cổng cảm biến — dùng để đọc bộ đếm chẩn đoán.
|
||||
const SensorGateway& sensors() const
|
||||
{
|
||||
return sensors_;
|
||||
}
|
||||
|
||||
// ==============================================================================================
|
||||
// robot::move_base_core::BaseNavigation
|
||||
// ==============================================================================================
|
||||
|
||||
void initialize(robot::TFListenerPtr tf) override;
|
||||
|
||||
void setRobotFootprint(const std::vector<robot_geometry_msgs::Point>& fprt) override;
|
||||
std::vector<robot_geometry_msgs::Point> getRobotFootprint() override;
|
||||
|
||||
void addStaticMap(const std::string& map_name, robot_nav_msgs::OccupancyGrid map) override;
|
||||
void addLaserScan(const std::string& laser_scan_name,
|
||||
robot_sensor_msgs::LaserScan laser_scan) override;
|
||||
void addPointCloud(const std::string& point_cloud_name,
|
||||
robot_sensor_msgs::PointCloud point_cloud) override;
|
||||
void addPointCloud2(const std::string& point_cloud2_name,
|
||||
robot_sensor_msgs::PointCloud2 point_cloud2) override;
|
||||
void addDepthCameraData(const std::string& topic,
|
||||
robot_sensor_msgs::DepthCameraData::ConstPtr data) override;
|
||||
|
||||
robot_nav_msgs::OccupancyGrid getStaticMap(const std::string& map_name) override;
|
||||
robot_sensor_msgs::LaserScan getLaserScan(const std::string& laser_scan_name) override;
|
||||
robot_sensor_msgs::PointCloud getPointCloud(const std::string& point_cloud_name) override;
|
||||
robot_sensor_msgs::PointCloud2 getPointCloud2(const std::string& point_cloud2_name) override;
|
||||
|
||||
std::map<std::string, robot_nav_msgs::OccupancyGrid> getAllStaticMaps() override;
|
||||
std::map<std::string, robot_sensor_msgs::LaserScan> getAllLaserScans() override;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud> getAllPointClouds() override;
|
||||
std::map<std::string, robot_sensor_msgs::PointCloud2> getAllPointCloud2s() override;
|
||||
|
||||
bool removeStaticMap(const std::string& map_name) override;
|
||||
bool removeLaserScan(const std::string& laser_scan_name) override;
|
||||
bool removePointCloud(const std::string& point_cloud_name) override;
|
||||
bool removePointCloud2(const std::string& point_cloud2_name) override;
|
||||
|
||||
bool removeAllStaticMaps() override;
|
||||
bool removeAllLaserScans() override;
|
||||
bool removeAllPointClouds() override;
|
||||
bool removeAllPointCloud2s() override;
|
||||
bool removeAllData() override;
|
||||
|
||||
void addOdometry(const std::string& odometry_name, robot_nav_msgs::Odometry odometry) override;
|
||||
|
||||
bool moveTo(const robot_geometry_msgs::PoseStamped& goal, double xy_goal_tolerance,
|
||||
double yaw_goal_tolerance) override;
|
||||
bool moveTo(const robot_protocol_msgs::Order& msg, const robot_geometry_msgs::PoseStamped& goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance) override;
|
||||
bool dockTo(const std::string& maker, const robot_geometry_msgs::PoseStamped& goal,
|
||||
double xy_goal_tolerance, double yaw_goal_tolerance) override;
|
||||
bool dockTo(const robot_protocol_msgs::Order& msg, const std::string& marker,
|
||||
const robot_geometry_msgs::PoseStamped& goal, double xy_goal_tolerance,
|
||||
double yaw_goal_tolerance) override;
|
||||
bool moveStraightTo(const robot_geometry_msgs::PoseStamped& goal,
|
||||
double xy_goal_tolerance) override;
|
||||
bool rotateTo(const robot_geometry_msgs::PoseStamped& goal, double yaw_goal_tolerance) override;
|
||||
|
||||
void pause() override;
|
||||
void resume() override;
|
||||
void cancel() override;
|
||||
|
||||
bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override;
|
||||
bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override;
|
||||
|
||||
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) override;
|
||||
bool getRobotPose(robot_geometry_msgs::Pose2D& pose) override;
|
||||
|
||||
robot_nav_2d_msgs::Twist2DStamped getTwist() override;
|
||||
|
||||
robot::move_base_core::NavFeedback* getFeedback() override;
|
||||
robot::move_base_core::PlannerDataOutput getGlobalData() override;
|
||||
robot::move_base_core::PlannerDataOutput getLocalData() override;
|
||||
|
||||
private:
|
||||
/// @brief Đường vào duy nhất của mọi lệnh di chuyển. Sáu entry point host chỉ dựng struct rồi gọi.
|
||||
bool submit(const NavigationRequest& request);
|
||||
|
||||
/// @brief Đồng bộ nav_feedback_ với state hiện tại của lõi.
|
||||
void refreshFeedback();
|
||||
|
||||
/**
|
||||
* @brief Đưa lệnh vận tốc của cycle vừa chạy ra @ref getTwist.
|
||||
*
|
||||
* `getTwist()` của contract host là **lệnh đang phát**, không phải vận tốc đo được: host lấy nó
|
||||
* publish thẳng ra cmd_vel. Nguồn duy nhất hợp lệ là đầu ra của VelocityArbiter.
|
||||
*/
|
||||
void publishCommand();
|
||||
|
||||
/**
|
||||
* @brief Đẩy trần vận tốc và vận tốc đo được mà host đã đặt xuống controller.
|
||||
*
|
||||
* Gọi từ @ref spinOnce, tức **control thread**. Host đặt các giá trị này từ thread của nó
|
||||
* (OPC-UA, VDA5050, ROS); `ControllerPort` không thread-safe nên chúng phải được cất lại rồi đẩy
|
||||
* xuống ở đây, không gọi thẳng.
|
||||
*/
|
||||
void pushHostInputsToController();
|
||||
|
||||
/// @brief Ánh xạ state của lõi sang enum trạng thái của contract host.
|
||||
static robot::move_base_core::State toHostState(NavigationState state);
|
||||
|
||||
ControlLoop loop_;
|
||||
SensorGateway sensors_;
|
||||
robot::TFListenerPtr tf_;
|
||||
|
||||
/// Bảo vệ dữ liệu sensor và footprint: host ghi từ thread của nó, control loop đọc.
|
||||
mutable std::mutex data_mutex_;
|
||||
|
||||
std::vector<robot_geometry_msgs::Point> footprint_;
|
||||
std::map<std::string, robot_sensor_msgs::DepthCameraData::ConstPtr> depth_camera_data_;
|
||||
|
||||
/// Frame đóng dấu lên lệnh vận tốc gửi host. Chép từ config lúc @ref configureLoop.
|
||||
std::string robot_base_frame_ = "base_link";
|
||||
|
||||
/**
|
||||
* Trần vận tốc host vừa đặt, chờ được đẩy xuống controller ở cycle kế tiếp.
|
||||
*
|
||||
* Đây là đường tầng an toàn hạ tốc độ robot (`amr_control.cpp:561, 671-680`), nên bỏ lỡ một lời
|
||||
* gọi là bỏ lỡ một yêu cầu giảm tốc. Giữ riêng tiến/lùi vì dấu chọn chiều.
|
||||
*/
|
||||
robot_geometry_msgs::Vector3 pending_linear_forward_;
|
||||
robot_geometry_msgs::Vector3 pending_linear_backward_;
|
||||
robot_geometry_msgs::Vector3 pending_angular_;
|
||||
bool has_pending_linear_forward_ = false;
|
||||
bool has_pending_linear_backward_ = false;
|
||||
bool has_pending_angular_ = false;
|
||||
|
||||
std::string last_reject_reason_;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_NAVIGATION_SERVER_H_
|
||||
92
include/move_base2/ports/action_port.h
Normal file
92
include/move_base2/ports/action_port.h
Normal file
@@ -0,0 +1,92 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng ra phía thực thi action của mission (D8).
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_ACTION_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_ACTION_PORT_H_
|
||||
|
||||
#include <string>
|
||||
|
||||
// robot_protocol_msgs/Action.h khai boost::shared_ptr nhưng không tự include nó. Translation unit
|
||||
// nào chạm Action.h trước một header boost khác sẽ hỏng, nên nạp ở đây — chỗ duy nhất trong gói
|
||||
// kéo Action.h vào.
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <robot_protocol_msgs/Action.h>
|
||||
|
||||
namespace robot
|
||||
{
|
||||
class NodeHandle;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct ActionTick
|
||||
* @brief Kết quả một control cycle của action đang chạy.
|
||||
*
|
||||
* Khác RecoveryTick, tick của action KHÔNG có vận tốc lẫn path: theo D8, trong lúc chạy action
|
||||
* không ai được phát cmd_vel — action cần chuyển động phải được mô hình hoá thành motion profile
|
||||
* của navigation. Contract này là hàng rào an toàn, không phải thiếu sót.
|
||||
*/
|
||||
struct ActionTick
|
||||
{
|
||||
enum class Status
|
||||
{
|
||||
kRunning,
|
||||
kSucceeded,
|
||||
kFailed
|
||||
};
|
||||
|
||||
Status status = Status::kRunning;
|
||||
|
||||
std::string message; ///< Mô tả người-đọc-được, chỉ để log khi state đổi.
|
||||
};
|
||||
|
||||
/**
|
||||
* @class ActionPort
|
||||
* @brief Cổng ra phía thực thi action. Tick-based: mỗi control cycle một lời gọi @ref update.
|
||||
*
|
||||
* Cùng mô hình với RecoveryPort và cùng lý do (D5): handler được tick từ control thread ở
|
||||
* `controller_frequency`, KHÔNG được block — handler chờ thiết bị (nâng kệ, sạc…) thì tự giữ state
|
||||
* và trả kRunning, nhờ vậy cancel/pause/emergency luôn được phản hồi trong một cycle.
|
||||
*
|
||||
* Contract timeout (3 tầng, tầng 1 là chính): (1) MỖI handler phải tự timeout theo hiểu biết
|
||||
* thiết bị của nó — "nâng kệ quá 20 s là bất thường" khác hẳn "sạc 30 phút là bình thường", chỉ
|
||||
* handler biết ngưỡng đúng, quá ngưỡng thì trả kFailed; (2) `action_patience` của state machine là
|
||||
* lưới cuối cho handler treo, mặc định tắt; (3) `mission_timeout` của mission layer đo cả chặng.
|
||||
* Handler trả kRunning vĩnh viễn mà không có đường thoát riêng là handler viết sai contract.
|
||||
*
|
||||
* Phase 4 hiện thực port này bằng ActionRunner: nạp ActionHandler plugin qua boost::dll +
|
||||
* `library_path`, route theo actionType VDA5050. Ở Phase 1 chỉ có fake cho test.
|
||||
*/
|
||||
class ActionPort
|
||||
{
|
||||
public:
|
||||
virtual ~ActionPort() = default;
|
||||
|
||||
/// @brief Nạp và cấu hình các action handler. Gọi một lần lúc khởi tạo.
|
||||
virtual bool configure(robot::NodeHandle& nh) = 0;
|
||||
|
||||
/**
|
||||
* @brief Bắt đầu một action.
|
||||
* @return false nếu không có handler nào nhận actionType này hoặc handler từ chối khởi động —
|
||||
* bên gọi coi như action thất bại, không được tick tiếp.
|
||||
*/
|
||||
virtual bool start(const robot_protocol_msgs::Action& action) = 0;
|
||||
|
||||
/// @brief Một control cycle. CHỈ được gọi sau khi @ref start trả true.
|
||||
virtual ActionTick update() = 0;
|
||||
|
||||
/// @brief Yêu cầu dừng an toàn action đang chạy (huỷ mission, emergency).
|
||||
virtual void cancel() = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_ACTION_PORT_H_
|
||||
46
include/move_base2/ports/clock_port.h
Normal file
46
include/move_base2/ports/clock_port.h
Normal file
@@ -0,0 +1,46 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng thời gian.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_CLOCK_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_CLOCK_PORT_H_
|
||||
|
||||
#include <robot/time.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class ClockPort
|
||||
* @brief Nguồn thời gian của runtime.
|
||||
*
|
||||
* Mọi ngưỡng thời gian của navigation (planner_patience, controller_patience, oscillation_timeout)
|
||||
* và mọi phép tích phân theo dt đều đi qua cổng này. Lý do: nếu lấy thẳng robot::Time::now() thì
|
||||
* không cách nào kiểm được hành vi khi control loop chạy chậm hơn chu kỳ cấu hình — đúng lớp lỗi
|
||||
* mà dead-reckoning theo chu kỳ cấu hình mắc phải.
|
||||
*/
|
||||
class ClockPort
|
||||
{
|
||||
public:
|
||||
virtual ~ClockPort() = default;
|
||||
|
||||
virtual robot::Time now() const = 0;
|
||||
};
|
||||
|
||||
/// @brief Cổng thời gian dùng ở runtime thật.
|
||||
class SystemClock final : public ClockPort
|
||||
{
|
||||
public:
|
||||
robot::Time now() const override
|
||||
{
|
||||
return robot::Time::now();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_CLOCK_PORT_H_
|
||||
97
include/move_base2/ports/controller_port.h
Normal file
97
include/move_base2/ports/controller_port.h
Normal file
@@ -0,0 +1,97 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng ra phía local planner (controller).
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_CONTROLLER_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_CONTROLLER_PORT_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
#include <robot_geometry_msgs/Vector3.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class ControllerPort
|
||||
* @brief Cổng ra phía local planner (thế hệ 1: robot_nav_core::BaseLocalPlanner).
|
||||
*
|
||||
* Giữ nguyên bộ hàm của interface được bọc — kể cả việc `isGoalReached()` tách khỏi
|
||||
* `computeVelocityCommands()`. Thứ tự gọi cũng giữ nguyên: hỏi đã tới đích trước, chỉ khi chưa mới
|
||||
* tính lệnh. Đổi thứ tự này sẽ đổi hành vi tại đích của mọi local planner đang chạy.
|
||||
*/
|
||||
class ControllerPort
|
||||
{
|
||||
public:
|
||||
virtual ~ControllerPort() = default;
|
||||
|
||||
/**
|
||||
* @brief Đổi local planner đang dùng theo profile của yêu cầu.
|
||||
* @return false nếu không nạp được — bên gọi phải từ chối yêu cầu.
|
||||
*/
|
||||
virtual bool swapPlanner(const std::string& planner_name) = 0;
|
||||
|
||||
/**
|
||||
* @brief Đặt sai số chấp nhận tại đích cho yêu cầu hiện tại.
|
||||
* @param xy_m [m]
|
||||
* @param yaw_rad [rad]
|
||||
*/
|
||||
virtual void setTolerance(double xy_m, double yaw_rad) = 0;
|
||||
|
||||
/**
|
||||
* @brief Nạp plan mới.
|
||||
* @return false nếu controller từ chối plan (rỗng, sai frame, không bám được).
|
||||
*/
|
||||
virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) = 0;
|
||||
|
||||
/**
|
||||
* @brief Tính lệnh vận tốc cho cycle này.
|
||||
* @param[out] cmd [m/s], [rad/s]. CHỈ hợp lệ khi hàm trả true.
|
||||
* @return false khi không sinh được lệnh hợp lệ ở cycle này.
|
||||
*/
|
||||
virtual bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) = 0;
|
||||
|
||||
/// @brief Đã tới đích theo sai số đã đặt hay chưa.
|
||||
virtual bool isGoalReached() = 0;
|
||||
|
||||
/**
|
||||
* @brief Vận tốc đo được của robot, dùng làm dữ liệu vào cho lần tính lệnh kế tiếp.
|
||||
*
|
||||
* Interface được bọc nhận vận tốc hiện tại như tham số của `computeVelocityCommands`; bản cũ lấy
|
||||
* nó từ `odometry_.twist.twist`. Truyền **theo giá trị** qua cổng này thay vì cho controller giữ
|
||||
* con trỏ tới bộ nhớ do host ghi — bản cũ làm thế (`tc_->setOdom(&odometry_)`) và đó là một data
|
||||
* race không có gì bảo vệ: host ghi từ thread của nó, control thread đọc qua con trỏ.
|
||||
*
|
||||
* @param velocity [m/s], [rad/s] trong hệ thân xe.
|
||||
*/
|
||||
virtual void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) = 0;
|
||||
|
||||
/**
|
||||
* @brief Đặt trần vận tốc thẳng. **Dấu chọn chiều**: dương = tiến, âm = lùi.
|
||||
*
|
||||
* Đây không phải lệnh jog dù tên nghe như vậy. Host gọi theo cặp `+v` rồi `-v` để đặt trần cho cả
|
||||
* hai chiều, và giá trị nó truyền xuống mang theo **tốc độ đã bị tầng an toàn hạ xuống**
|
||||
* (`amr_control.cpp:671-680`). Bỏ qua lời gọi này nghĩa là tầng an toàn yêu cầu giảm tốc mà robot
|
||||
* vẫn chạy nguyên tốc độ planner.
|
||||
*
|
||||
* @return false nếu controller đang dùng không hỗ trợ đặt trần.
|
||||
*/
|
||||
virtual bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) = 0;
|
||||
|
||||
/// @brief Đặt trần vận tốc góc. Quy ước dấu như @ref setTwistLinear.
|
||||
virtual bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) = 0;
|
||||
|
||||
/// @brief Tên controller đang hoạt động; chuỗi rỗng nếu chưa nạp được.
|
||||
virtual std::string activeController() const = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_CONTROLLER_PORT_H_
|
||||
70
include/move_base2/ports/mission_port.h
Normal file
70
include/move_base2/ports/mission_port.h
Normal file
@@ -0,0 +1,70 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng ra phía mission layer.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_MISSION_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_MISSION_PORT_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
|
||||
#include <move_base2/core/navigation_request.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/// @brief Kết quả của một chặng navigation, theo ngôn ngữ mà mission layer cần để chạy tiếp hàng đợi.
|
||||
enum class NavigationOutcome
|
||||
{
|
||||
kSucceeded,
|
||||
kFailed,
|
||||
kCancelled,
|
||||
kPreempted
|
||||
};
|
||||
|
||||
/// @brief Tên outcome dạng chuỗi.
|
||||
const char* toString(NavigationOutcome outcome);
|
||||
|
||||
/**
|
||||
* @class MissionPort
|
||||
* @brief Cổng ra phía mission. move_base2 KHÔNG biết mission framework nào đang chạy phía sau.
|
||||
*
|
||||
* Luồng một chiều, không polling: mission layer đẩy chặng xuống qua callback đã đăng ký; move_base2
|
||||
* báo ngược kết quả qua @ref reportOutcome. Bản thế hệ 1 không có khái niệm này nên phía mission
|
||||
* phải poll trường trạng thái của navigation — cách đó dễ trượt sự kiện hoặc đếm hai lần khi state
|
||||
* đổi nhanh hơn nhịp poll.
|
||||
*
|
||||
* @invariant @ref reportOutcome chỉ được gọi ĐÚNG MỘT LẦN cho mỗi
|
||||
* NavigationRequest::mission_sequence_id khác 0.
|
||||
*/
|
||||
class MissionPort
|
||||
{
|
||||
public:
|
||||
using RequestCallback = std::function<void(const NavigationRequest&)>;
|
||||
|
||||
virtual ~MissionPort() = default;
|
||||
|
||||
/// @brief Đăng ký callback mà mission layer gọi mỗi khi có chặng mới cần chạy.
|
||||
virtual void setRequestCallback(RequestCallback callback) = 0;
|
||||
|
||||
/**
|
||||
* @brief Báo kết quả chặng vừa chạy.
|
||||
* @param mission_sequence_id Echo lại từ NavigationRequest.
|
||||
* @param outcome Kết quả chặng.
|
||||
*/
|
||||
virtual void reportOutcome(std::uint64_t mission_sequence_id, NavigationOutcome outcome) = 0;
|
||||
|
||||
/// @brief Có mission đang chạy hay không. Chỉ dùng để publish feedback, KHÔNG dùng để điều khiển.
|
||||
virtual bool hasActiveMission() const = 0;
|
||||
|
||||
virtual void start() = 0;
|
||||
virtual void stop() = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_MISSION_PORT_H_
|
||||
109
include/move_base2/ports/planner_port.h
Normal file
109
include/move_base2/ports/planner_port.h
Normal file
@@ -0,0 +1,109 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng ra phía global planner.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_PLANNER_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_PLANNER_PORT_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct PlanResult
|
||||
* @brief Kết quả một lượt lập plan.
|
||||
*/
|
||||
struct PlanResult
|
||||
{
|
||||
/**
|
||||
* @brief Nhãn của yêu cầu sinh ra kết quả này, chép từ @ref PlannerPort::startPlan.
|
||||
*
|
||||
* Bên gọi **phải** so nhãn này với yêu cầu đang chạy và vứt kết quả không khớp. Lập plan mất hàng
|
||||
* trăm ms; trong khoảng đó goal có thể đã đổi, và bám theo một plan tới goal cũ nghĩa là robot đi
|
||||
* tới chỗ không ai yêu cầu.
|
||||
*/
|
||||
std::uint64_t tag = 0;
|
||||
|
||||
/// Lượt lập plan có ra được plan không rỗng hay không.
|
||||
bool succeeded = false;
|
||||
|
||||
/// Plan trong global frame. Chỉ có nghĩa khi @ref succeeded.
|
||||
std::vector<robot_geometry_msgs::PoseStamped> plan;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class PlannerPort
|
||||
* @brief Cổng ra phía global planner (thế hệ 1: robot_nav_core::BaseGlobalPlanner).
|
||||
*
|
||||
* Contract **bất đồng bộ**: `startPlan` trả về ngay, kết quả lấy sau bằng `pollPlan`. Lý do là ràng
|
||||
* buộc thời gian thật chứ không phải sở thích kiến trúc — global planner nặng (SBPL lattice) mất
|
||||
* hàng trăm ms, trong khi control loop chạy 20 Hz và là thread **duy nhất** được phát `cmd_vel`.
|
||||
* Lập plan đồng bộ nghĩa là mỗi lần lập lại plan là ngần ấy thời gian robot chạy bằng lệnh cũ mà
|
||||
* không ai giám sát.
|
||||
*
|
||||
* Hai overload `makePlan` của interface gốc được gộp lại: "có Order hay không" chỉ là một nhánh nhỏ
|
||||
* bên trong hiện thực, không đáng nhân đôi contract.
|
||||
*/
|
||||
class PlannerPort
|
||||
{
|
||||
public:
|
||||
virtual ~PlannerPort() = default;
|
||||
|
||||
/**
|
||||
* @brief Đổi global planner đang dùng.
|
||||
* @param planner_name Tên alias plugin, khớp key trong config.
|
||||
* @return false nếu không nạp được — bên gọi phải từ chối yêu cầu, không đi tiếp với planner cũ.
|
||||
*/
|
||||
virtual bool swapPlanner(const std::string& planner_name) = 0;
|
||||
|
||||
/**
|
||||
* @brief Khởi động một lượt lập plan. **Không chặn.**
|
||||
*
|
||||
* @param order Order gốc nếu yêu cầu đến từ giao thức fleet; nullptr nếu là goal trực tiếp.
|
||||
* Hiện thực **phải sao chép** nội dung — con trỏ chỉ hợp lệ trong lời gọi này, còn
|
||||
* lượt lập plan sống lâu hơn thế.
|
||||
* @param tag Nhãn phân biệt yêu cầu, trả lại nguyên vẹn trong @ref PlanResult::tag.
|
||||
*
|
||||
* @return false nếu không khởi động được: chưa có planner, hoặc đang có một lượt chạy dở.
|
||||
*/
|
||||
virtual bool startPlan(const robot_geometry_msgs::PoseStamped& start,
|
||||
const robot_geometry_msgs::PoseStamped& goal,
|
||||
const robot_protocol_msgs::Order* order, std::uint64_t tag) = 0;
|
||||
|
||||
/// @brief Có lượt lập plan nào đang chạy không.
|
||||
virtual bool isPlanning() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Lấy kết quả nếu đã có. Không chặn.
|
||||
*
|
||||
* @param[out] result Chỉ được ghi khi hàm trả true. Hiện thực nên **hoán vị** vector plan với
|
||||
* `result.plan` thay vì copy, để bộ nhớ được tái sử dụng qua các lượt.
|
||||
* @return true khi có kết quả — kể cả kết quả thất bại. false nghĩa là chưa xong hoặc không có gì.
|
||||
*/
|
||||
virtual bool pollPlan(PlanResult& result) = 0;
|
||||
|
||||
/**
|
||||
* @brief Bỏ lượt đang chạy.
|
||||
*
|
||||
* @note Không cắt ngang được plugin đang tính: nó là hộp đen nạp lúc chạy, không có đường ngắt.
|
||||
* Huỷ ở đây nghĩa là **vứt kết quả khi nó về**, không phải dừng phép tính.
|
||||
*/
|
||||
virtual void cancelPlan() = 0;
|
||||
|
||||
/// @brief Tên planner đang hoạt động; chuỗi rỗng nếu chưa nạp được planner nào.
|
||||
virtual std::string activePlanner() const = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_PLANNER_PORT_H_
|
||||
38
include/move_base2/ports/pose_port.h
Normal file
38
include/move_base2/ports/pose_port.h
Normal file
@@ -0,0 +1,38 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng lấy pose robot.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_POSE_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_POSE_PORT_H_
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class PosePort
|
||||
* @brief Nguồn pose robot trong global frame.
|
||||
*
|
||||
* Ở runtime thật, hiện thực bọc Costmap2DROBOT::getRobotPose (đã bao gồm tra TF và kiểm
|
||||
* transform_tolerance). Trong test, hiện thực là fake bơm pose theo kịch bản.
|
||||
*
|
||||
* @invariant Trả false nghĩa là "không biết robot đang ở đâu" — TF thiếu, TF quá hạn, hoặc frame
|
||||
* chưa sẵn sàng. Bên gọi PHẢI dừng an toàn, không được dùng pose cũ để đi tiếp.
|
||||
* @p pose không được ghi khi hàm trả false.
|
||||
*/
|
||||
class PosePort
|
||||
{
|
||||
public:
|
||||
virtual ~PosePort() = default;
|
||||
|
||||
virtual bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_POSE_PORT_H_
|
||||
135
include/move_base2/ports/recovery_port.h
Normal file
135
include/move_base2/ports/recovery_port.h
Normal file
@@ -0,0 +1,135 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — cổng ra phía recovery.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|
||||
#define MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
|
||||
namespace robot
|
||||
{
|
||||
class NodeHandle;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/// @brief Lý do vào recovery. Behavior có thể dùng để chọn chiến lược khác nhau cho cùng một plugin.
|
||||
enum class RecoveryTrigger
|
||||
{
|
||||
kPlanningFailed, ///< Không lập được plan trong thời gian cho phép.
|
||||
kControllingFailed, ///< Không sinh được lệnh vận tốc hợp lệ trong thời gian cho phép.
|
||||
kOscillation ///< Robot quẩn tại chỗ quá lâu.
|
||||
};
|
||||
|
||||
const char* toString(RecoveryTrigger trigger);
|
||||
|
||||
/**
|
||||
* @enum RecoveryOutputKind
|
||||
* @brief Behavior đó có lái robot hay không.
|
||||
*
|
||||
* Lõi cần biết điều này **trước** khi behavior chạy tick đầu tiên: nếu behavior không phát vận tốc
|
||||
* (đợi, xoá costmap) thì nguồn vận tốc của cycle phải là `kNone`, không phải `kRecovery`. Coi mọi
|
||||
* behavior là nguồn vận tốc khiến `VelocityArbiter` đổi nguồn hai lần cho mỗi lượt one-shot, mỗi
|
||||
* lần chèn một cycle zero — vài chục ms cmd_vel = 0 không vì lý do gì.
|
||||
*
|
||||
* Khai riêng ở đây thay vì dùng `recovery_core::RecoveryOutputType`: lõi không được include
|
||||
* recovery framework (D3). `RecoveryRunner` map 1:1 hai enum bằng `switch`.
|
||||
*/
|
||||
enum class RecoveryOutputKind
|
||||
{
|
||||
kNone, ///< Không phát output (đợi, xoá costmap).
|
||||
kVelocity, ///< Phát Twist mỗi cycle (rotate, back up).
|
||||
kPath ///< Sinh đường đi mới.
|
||||
};
|
||||
|
||||
const char* toString(RecoveryOutputKind kind);
|
||||
|
||||
/**
|
||||
* @struct RecoveryTick
|
||||
* @brief Kết quả một control cycle của recovery, đã chuẩn hoá về ngôn ngữ của move_base2.
|
||||
*
|
||||
* Chuẩn hoá ở đây thay vì dùng thẳng kiểu của recovery framework là có chủ đích: lõi không được
|
||||
* include recovery framework, nếu không chiều phụ thuộc sẽ khoá cứng move_base2 vào một hiện thực.
|
||||
*
|
||||
* Bất biến đọc: chỉ đọc @ref cmd khi @ref has_velocity, chỉ đọc @ref path khi @ref has_path. Một
|
||||
* tick không bao giờ vừa có vận tốc vừa có path.
|
||||
*/
|
||||
struct RecoveryTick
|
||||
{
|
||||
enum class Status
|
||||
{
|
||||
kRunning,
|
||||
kSucceeded,
|
||||
kFailed
|
||||
};
|
||||
|
||||
Status status = Status::kRunning;
|
||||
|
||||
bool has_velocity = false; ///< true -> @ref cmd hợp lệ.
|
||||
robot_geometry_msgs::Twist cmd; ///< [m/s], [rad/s]. Dấu âm của linear.x nghĩa là lùi.
|
||||
|
||||
bool has_path = false; ///< true -> @ref path hợp lệ (họ recovery sinh lại đường đi).
|
||||
std::vector<robot_geometry_msgs::PoseStamped> path;
|
||||
|
||||
std::string message; ///< Mô tả người-đọc-được, chỉ để log khi state đổi.
|
||||
};
|
||||
|
||||
/**
|
||||
* @class RecoveryPort
|
||||
* @brief Cổng ra phía recovery. Tick-based: mỗi control cycle một lời gọi @ref update.
|
||||
*
|
||||
* Đây là khác biệt kiến trúc so với thế hệ 1, không phải đổi tên hàm. Recovery thế hệ 1 chạy
|
||||
* blocking bên trong một lời gọi và không trả gì; recovery thế hệ 2 trả kết quả từng cycle và có
|
||||
* thể phát vận tốc. Hệ quả bắt buộc: recovery phải được tick từ đúng thread đang sở hữu cmd_vel,
|
||||
* không được có thread riêng — hai thread cùng phát vận tốc là hai bộ điều khiển tranh nhau.
|
||||
*/
|
||||
class RecoveryPort
|
||||
{
|
||||
public:
|
||||
virtual ~RecoveryPort() = default;
|
||||
|
||||
/// @brief Nạp và cấu hình danh sách behavior. Gọi một lần lúc khởi tạo.
|
||||
virtual bool configure(robot::NodeHandle& nh) = 0;
|
||||
|
||||
/// @brief Số behavior đã nạp được. 0 nghĩa là không có đường phục hồi nào.
|
||||
virtual std::size_t behaviorCount() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Họ output của behavior thứ @p index — behavior đó có lái robot hay không.
|
||||
*
|
||||
* Được hỏi **trước** khi behavior chạy, kể cả ở cycle khởi động nó. Index sai phải trả
|
||||
* @ref RecoveryOutputKind::kNone (giả định an toàn: không cấp quyền phát vận tốc cho thứ không
|
||||
* biết là gì).
|
||||
*/
|
||||
virtual RecoveryOutputKind outputKind(std::size_t index) const = 0;
|
||||
|
||||
/**
|
||||
* @brief Bắt đầu behavior thứ @p index.
|
||||
* @return false nếu index sai hoặc behavior từ chối khởi động (ví dụ đã va chạm ngay tại chỗ).
|
||||
*/
|
||||
virtual bool start(std::size_t index, RecoveryTrigger trigger) = 0;
|
||||
|
||||
/// @brief Một control cycle. CHỈ được gọi sau khi @ref start trả true.
|
||||
virtual RecoveryTick update() = 0;
|
||||
|
||||
/// @brief Yêu cầu dừng. Tick kế tiếp phải trả kết quả dừng an toàn.
|
||||
virtual void cancel() = 0;
|
||||
|
||||
/// @brief Tên behavior thứ @p index; chuỗi rỗng nếu index sai.
|
||||
virtual std::string behaviorName(std::size_t index) const = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_PORTS_RECOVERY_PORT_H_
|
||||
79
include/move_base2/runners/action_handler.h
Normal file
79
include/move_base2/runners/action_handler.h
Normal file
@@ -0,0 +1,79 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — contract cho plugin thực thi một loại action.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
|
||||
#define MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// robot_protocol_msgs/Action.h khai boost::shared_ptr nhưng không tự include — phải nạp trước nó,
|
||||
// nếu không translation unit nào include Action.h đầu tiên sẽ hỏng.
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot/time.h>
|
||||
#include <robot_protocol_msgs/Action.h>
|
||||
|
||||
#include <move_base2/ports/action_port.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class ActionHandler
|
||||
* @brief Thực thi một hoặc nhiều loại action (`actionType` của VDA5050).
|
||||
*
|
||||
* Tick-based, cùng mô hình với recovery behavior và cùng lý do (D5): handler được gọi từ **control
|
||||
* thread** ở `controller_frequency`, nên nó **không được block**. Chờ thiết bị thì giữ state nội bộ
|
||||
* và trả `kRunning`; nhờ vậy cancel/pause/emergency luôn được phản hồi trong một cycle.
|
||||
*
|
||||
* @warning **Handler phải tự timeout.** Đây là tầng 1 của contract 3 tầng và là tầng chính: chỉ
|
||||
* handler biết ngưỡng đúng cho thiết bị của nó ("nâng kệ quá 20 s là bất thường" khác hẳn
|
||||
* "sạc 30 phút là bình thường"). `action_patience` của state machine chỉ là lưới cuối và
|
||||
* mặc định tắt. Một handler trả `kRunning` vĩnh viễn là handler viết sai contract.
|
||||
*
|
||||
* @note Trong lúc action chạy, **không ai được phát cmd_vel** (D8). Action cần chuyển động phải
|
||||
* được mô hình hoá thành motion profile của navigation, không phải làm trong handler.
|
||||
*/
|
||||
class ActionHandler
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<ActionHandler>;
|
||||
|
||||
virtual ~ActionHandler() = default;
|
||||
|
||||
/**
|
||||
* @brief Cấu hình một lần.
|
||||
* @param name Tên instance, dùng cho log.
|
||||
* @param nh NodeHandle **đã được caller scope sẵn** vào namespace param của instance này.
|
||||
* @return false nếu không chạy được với cấu hình này.
|
||||
*/
|
||||
virtual bool configure(const std::string& name, robot::NodeHandle& nh) = 0;
|
||||
|
||||
/// @brief Các `actionType` mà handler này nhận. Rỗng = không nhận gì (registry sẽ từ chối nạp).
|
||||
virtual std::vector<std::string> supportedActionTypes() const = 0;
|
||||
|
||||
/**
|
||||
* @brief Bắt đầu thực thi @p action.
|
||||
* @param now Thời điểm hiện tại — mốc cho timeout của chính handler.
|
||||
* @return false = từ chối khởi động; bên gọi coi như action thất bại và **không** tick tiếp.
|
||||
*/
|
||||
virtual bool start(const robot_protocol_msgs::Action& action, const robot::Time& now) = 0;
|
||||
|
||||
/// @brief Một control cycle. Chỉ được gọi sau khi @ref start trả true.
|
||||
virtual ActionTick update(const robot::Time& now) = 0;
|
||||
|
||||
/// @brief Yêu cầu dừng an toàn. Handler phải đưa thiết bị về trạng thái an toàn, không treo.
|
||||
virtual void cancel() = 0;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_RUNNERS_ACTION_HANDLER_H_
|
||||
112
include/move_base2/runners/action_runner.h
Normal file
112
include/move_base2/runners/action_runner.h
Normal file
@@ -0,0 +1,112 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — hiện thực ActionPort bằng các ActionHandler plugin.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_
|
||||
#define MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <move_base2/ports/action_port.h>
|
||||
#include <move_base2/ports/clock_port.h>
|
||||
#include <move_base2/runners/action_handler.h>
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class ActionRunner
|
||||
* @brief Bảng tra `actionType` -> handler, nạp từ YAML bằng Boost.DLL.
|
||||
*
|
||||
* Cấu hình mong đợi:
|
||||
*
|
||||
* @code{.yaml}
|
||||
* actions:
|
||||
* handlers:
|
||||
* - {name: noop, type: NoopActionHandler}
|
||||
* noop:
|
||||
* action_types: [wait, pick, drop]
|
||||
* duration: 0.0 # [s]
|
||||
*
|
||||
* NoopActionHandler:
|
||||
* library_path: libmove_base2_noop_action_handler
|
||||
* @endcode
|
||||
*
|
||||
* Danh sách handler **được phép rỗng**: một hệ không có thiết bị nào thì mọi mission đều là
|
||||
* nav-only, và khi đó `ControlLoop::submit` đã từ chối yêu cầu mang action ngay tại cửa.
|
||||
*
|
||||
* @note Không thread-safe. Chỉ control thread được gọi.
|
||||
*/
|
||||
class ActionRunner final : public ActionPort
|
||||
{
|
||||
public:
|
||||
ActionRunner() = default;
|
||||
~ActionRunner() override;
|
||||
|
||||
ActionRunner(const ActionRunner&) = delete;
|
||||
ActionRunner& operator=(const ActionRunner&) = delete;
|
||||
|
||||
/// @brief Đồng hồ runtime. Bắt buộc đặt trước @ref configure.
|
||||
void setClock(ClockPort* clock);
|
||||
|
||||
/// @brief Namespace YAML chứa `<ns>/handlers`. Mặc định "actions".
|
||||
void setNamespace(const std::string& ns);
|
||||
|
||||
/**
|
||||
* @brief Đăng ký một handler dựng sẵn (test, hoặc handler biên dịch thẳng vào host).
|
||||
* @return false nếu handler null, không khai `actionType` nào, hoặc trùng type đã có chủ.
|
||||
*/
|
||||
bool registerHandler(const ActionHandler::Ptr& handler);
|
||||
|
||||
bool configure(robot::NodeHandle& nh) override;
|
||||
bool start(const robot_protocol_msgs::Action& action) override;
|
||||
ActionTick update() override;
|
||||
void cancel() override;
|
||||
|
||||
/// @brief Số handler đã nạp.
|
||||
std::size_t handlerCount() const
|
||||
{
|
||||
return handlers_.size();
|
||||
}
|
||||
|
||||
/// @brief Các `actionType` đã có handler nhận. Dùng cho log và test.
|
||||
std::vector<std::string> supportedActionTypes() const;
|
||||
|
||||
/// @brief Handler nhận @p action_type, hoặc nullptr.
|
||||
ActionHandler* find(const std::string& action_type) const;
|
||||
|
||||
private:
|
||||
/// Nạp một handler. Trả false kèm log lý do nếu hỏng ở bất kỳ bước nào.
|
||||
bool loadOne(const std::string& name, const std::string& type, robot::NodeHandle& nh,
|
||||
const std::string& ns);
|
||||
|
||||
ClockPort* clock_ = nullptr; ///< non-owning
|
||||
std::string namespace_ = "actions";
|
||||
bool configured_ = false;
|
||||
|
||||
std::vector<ActionHandler::Ptr> handlers_;
|
||||
std::map<std::string, ActionHandler*> by_type_; ///< non-owning, trỏ vào handlers_
|
||||
|
||||
ActionHandler* active_ = nullptr; ///< non-owning
|
||||
std::string active_action_id_;
|
||||
|
||||
/**
|
||||
* Giữ factory của Boost.DLL sống đúng bằng vòng đời runner.
|
||||
*
|
||||
* Không phải biến thừa: factory nắm `shared_library` bên trong, thả nó ra là `.so` bị unload
|
||||
* trong khi handler tạo từ nó vẫn còn sống — vtable trỏ vào vùng đã gỡ.
|
||||
*/
|
||||
std::vector<std::function<ActionHandler::Ptr()>> factories_;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_RUNNERS_ACTION_RUNNER_H_
|
||||
157
include/move_base2/runners/controller_runner.h
Normal file
157
include/move_base2/runners/controller_runner.h
Normal file
@@ -0,0 +1,157 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — hiện thực ControllerPort bằng plugin robot_nav_core::BaseLocalPlanner.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_
|
||||
#define MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot_nav_core/base_local_planner.h>
|
||||
|
||||
#include <move_base2/ports/controller_port.h>
|
||||
|
||||
namespace robot_costmap_2d
|
||||
{
|
||||
class Costmap2DROBOT;
|
||||
}
|
||||
namespace tf3
|
||||
{
|
||||
class BufferCore;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class ControllerRunner
|
||||
* @brief Nạp và chạy local planner thế hệ 1 (`robot_nav_core::BaseLocalPlanner`) qua Boost.DLL.
|
||||
*
|
||||
* Đây là chỗ duy nhất trong gói biết tới `robot_nav_core::BaseLocalPlanner`. Lõi quyết định chỉ thấy
|
||||
* @ref ControllerPort.
|
||||
*
|
||||
* ## Chạy đồng bộ, có chủ đích
|
||||
*
|
||||
* Khác @ref PlannerRunner, controller **không** cần thread riêng: nó phải trả lệnh trong đúng cycle
|
||||
* hiện tại. Một local planner mất hơn một chu kỳ điều khiển là lỗi cấu hình của chính nó, không phải
|
||||
* thứ kiến trúc ở đây che đi được — che đi sẽ thành robot chạy bằng lệnh cũ mà không ai biết.
|
||||
*
|
||||
* ## Sai số tại đích đi qua param server
|
||||
*
|
||||
* `BaseLocalPlanner` **không có** hàm đặt sai số. Bản cũ đặt qua `NodeHandle::setParam` và để
|
||||
* planner tự đọc lại (`move_base.cpp:1897-1907`). Kênh đó gián tiếp và mong manh — planner nào đọc
|
||||
* param một lần lúc `initialize` sẽ không bao giờ thấy giá trị mới — nhưng đổi nó là đổi interface
|
||||
* gen-1, ảnh hưởng mọi planner đang chạy. Giữ nguyên, và ghi lại ở đây để không ai tưởng nó chắc
|
||||
* chắn có tác dụng.
|
||||
*
|
||||
* ## Trần vận tốc (`setTwistLinear`)
|
||||
*
|
||||
* Tên nghe như lệnh jog nhưng thực chất là đặt trần, dấu chọn chiều. Đường này mang **tốc độ đã bị
|
||||
* tầng an toàn hạ xuống**; bỏ qua nó là bỏ qua yêu cầu giảm tốc của tầng an toàn.
|
||||
*
|
||||
* @note **Không thread-safe — mọi hàm chỉ được gọi từ control thread.**
|
||||
* Host đặt trần vận tốc và bơm odometry từ thread của nó (OPC-UA, VDA5050, ROS), nên
|
||||
* `NavigationServer` phải cất các giá trị đó lại và đẩy xuống đây trong `spinOnce()`. Gọi
|
||||
* thẳng từ thread host sẽ chạm `active_` và chạm plugin song song với lúc control thread đang
|
||||
* tính lệnh.
|
||||
*/
|
||||
class ControllerRunner : public ControllerPort
|
||||
{
|
||||
public:
|
||||
ControllerRunner();
|
||||
~ControllerRunner() override;
|
||||
|
||||
ControllerRunner(const ControllerRunner&) = delete;
|
||||
ControllerRunner& operator=(const ControllerRunner&) = delete;
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình và controller khởi đầu.
|
||||
*
|
||||
* @param nh NodeHandle để tra `library_path` và để đặt sai số tại đích. Được
|
||||
* **sao chép** vì cả hai việc đó xảy ra lúc chạy.
|
||||
* @param tf Buffer TF truyền cho `BaseLocalPlanner::initialize`. **Non-owning**.
|
||||
* @param costmap Costmap local. **Non-owning**, bắt buộc khác null.
|
||||
* @param initial_controller Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner.
|
||||
* @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool configure(const robot::NodeHandle& nh, tf3::BufferCore* tf,
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap,
|
||||
const std::string& initial_controller, std::string& error);
|
||||
|
||||
bool configured() const
|
||||
{
|
||||
return configured_;
|
||||
}
|
||||
|
||||
// ================================================================================================
|
||||
// ControllerPort
|
||||
// ================================================================================================
|
||||
|
||||
bool swapPlanner(const std::string& planner_name) override;
|
||||
void setTolerance(double xy_m, double yaw_rad) override;
|
||||
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>& plan) override;
|
||||
bool computeVelocityCommands(robot_geometry_msgs::Twist& cmd) override;
|
||||
bool isGoalReached() override;
|
||||
void setMeasuredVelocity(const robot_geometry_msgs::Twist& velocity) override;
|
||||
bool setTwistLinear(const robot_geometry_msgs::Vector3& linear) override;
|
||||
bool setTwistAngular(const robot_geometry_msgs::Vector3& angular) override;
|
||||
std::string activeController() const override;
|
||||
|
||||
/// @brief Số plugin đã nạp và còn giữ trong cache.
|
||||
std::size_t loadedCount() const
|
||||
{
|
||||
return controllers_.size();
|
||||
}
|
||||
|
||||
private:
|
||||
/// @brief Một plugin đã nạp: factory phải sống cùng instance — vứt factory là để `.so` unload.
|
||||
struct Loaded
|
||||
{
|
||||
std::function<robot_nav_core::BaseLocalPlanner::Ptr()> factory;
|
||||
robot_nav_core::BaseLocalPlanner::Ptr instance;
|
||||
};
|
||||
|
||||
/// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do).
|
||||
robot_nav_core::BaseLocalPlanner* acquire(const std::string& name);
|
||||
|
||||
/// @brief Áp lại trần vận tốc và sai số đã lưu lên controller vừa đổi sang.
|
||||
void applyPendingLimits(robot_nav_core::BaseLocalPlanner* controller);
|
||||
|
||||
robot::NodeHandle nh_;
|
||||
tf3::BufferCore* tf_ = nullptr;
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
|
||||
bool configured_ = false;
|
||||
|
||||
std::map<std::string, Loaded> controllers_;
|
||||
std::string active_name_;
|
||||
robot_nav_core::BaseLocalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref controllers_.
|
||||
|
||||
/**
|
||||
* Trần vận tốc và vận tốc đo được gần nhất.
|
||||
*
|
||||
* Giữ lại bản sao vì hai lý do: host đặt trần **trước** khi controller được nạp (thứ tự khởi tạo
|
||||
* không do move_base2 quyết), và @ref swapPlanner đổi sang một instance chưa biết gì về các trần
|
||||
* đã đặt — không áp lại là robot lặng lẽ chạy nhanh hơn mức tầng an toàn cho phép.
|
||||
*/
|
||||
robot_geometry_msgs::Vector3 limit_linear_forward_;
|
||||
robot_geometry_msgs::Vector3 limit_linear_backward_;
|
||||
robot_geometry_msgs::Vector3 limit_angular_;
|
||||
bool has_limit_linear_forward_ = false;
|
||||
bool has_limit_linear_backward_ = false;
|
||||
bool has_limit_angular_ = false;
|
||||
|
||||
robot_geometry_msgs::Twist measured_velocity_;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_RUNNERS_CONTROLLER_RUNNER_H_
|
||||
176
include/move_base2/runners/planner_runner.h
Normal file
176
include/move_base2/runners/planner_runner.h
Normal file
@@ -0,0 +1,176 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — hiện thực PlannerPort bằng plugin robot_nav_core::BaseGlobalPlanner, chạy trên
|
||||
* thread riêng.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|
||||
#define MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|
||||
|
||||
#include <condition_variable>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot_nav_core/base_global_planner.h>
|
||||
|
||||
#include <move_base2/ports/planner_port.h>
|
||||
|
||||
namespace robot_costmap_2d
|
||||
{
|
||||
class Costmap2DROBOT;
|
||||
}
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class PlannerRunner
|
||||
* @brief Nạp và chạy global planner thế hệ 1 (`robot_nav_core::BaseGlobalPlanner`) qua Boost.DLL,
|
||||
* trên một thread riêng.
|
||||
*
|
||||
* Đây là chỗ duy nhất trong gói biết tới `robot_nav_core`. Lõi quyết định chỉ thấy @ref PlannerPort.
|
||||
*
|
||||
* ## Vì sao có thread riêng
|
||||
*
|
||||
* Global planner nặng mất hàng trăm ms; control loop chạy 20 Hz và là thread duy nhất phát
|
||||
* `cmd_vel`. Lập plan tại chỗ nghĩa là mỗi lần lập lại plan là ngần ấy thời gian robot chạy bằng
|
||||
* lệnh cũ không ai giám sát.
|
||||
*
|
||||
* ## Bàn giao plan bằng hoán vị, không copy
|
||||
*
|
||||
* Một plan toàn cục có thể vài nghìn pose. Copy nó mỗi lượt là cấp phát lớn trên đường nóng. Ở đây
|
||||
* có **ba** vector luân chuyển bằng `swap`, đúng mô hình triple buffer của bản cũ nhưng bằng giá
|
||||
* trị thay vì con trỏ thô:
|
||||
*
|
||||
* ```
|
||||
* planning_ thread planner ghi vào
|
||||
* handoff_ hộp thư, đổi dưới mutex
|
||||
* (của bên gọi) pollPlan hoán vị với handoff_ -> vector cũ của bên gọi quay lại làm hộp thư
|
||||
* ```
|
||||
*
|
||||
* Mutex chỉ bị giữ trong lúc đổi vector, không bao giờ trong lúc plugin đang tính.
|
||||
*
|
||||
* ## Nhãn yêu cầu (tag)
|
||||
*
|
||||
* Lượt lập plan sống lâu hơn cái goal sinh ra nó. Mỗi lượt mang một nhãn do bên gọi cấp; bên gọi so
|
||||
* nhãn và vứt kết quả không khớp. Không có nó thì một plan tới goal đã bị huỷ vẫn được bám theo.
|
||||
*
|
||||
* ## Giữ instance sống bằng cách nào
|
||||
*
|
||||
* `boost::dll::import_alias` trả về factory **giữ tham chiếu tới thư viện đã nạp**. Vứt factory đi
|
||||
* trong khi instance nó tạo ra còn sống là để `.so` bị unload dưới chân object. Mỗi entry vì thế
|
||||
* giữ **cả hai**. Cache cũng **không bao giờ xoá** entry — nhờ đó con trỏ planner mà thread đang
|
||||
* dùng vẫn hợp lệ kể cả khi @ref swapPlanner đổi sang planner khác giữa chừng.
|
||||
*
|
||||
* @note Các hàm public gọi từ control thread. Thread nội bộ chỉ chạm dữ liệu dưới mutex.
|
||||
* @warning Destructor **chờ** lượt đang chạy kết thúc: plugin là hộp đen, không có đường cắt ngang.
|
||||
*/
|
||||
class PlannerRunner : public PlannerPort
|
||||
{
|
||||
public:
|
||||
PlannerRunner();
|
||||
~PlannerRunner() override;
|
||||
|
||||
PlannerRunner(const PlannerRunner&) = delete;
|
||||
PlannerRunner& operator=(const PlannerRunner&) = delete;
|
||||
|
||||
/**
|
||||
* @brief Nạp cấu hình, planner khởi đầu, và khởi động thread.
|
||||
*
|
||||
* @param nh NodeHandle để tra `library_path`. Được **sao chép** vì @ref swapPlanner
|
||||
* cần tra lại lúc chạy.
|
||||
* @param costmap Costmap global truyền cho `BaseGlobalPlanner::initialize`. **Non-owning**,
|
||||
* bắt buộc khác null, phải sống lâu hơn object này.
|
||||
* @param initial_planner Alias plugin nạp ngay. Chuỗi rỗng = chờ @ref swapPlanner.
|
||||
* @param[out] error Lý do thất bại; chỉ ghi khi hàm trả false.
|
||||
*/
|
||||
bool configure(const robot::NodeHandle& nh, robot_costmap_2d::Costmap2DROBOT* costmap,
|
||||
const std::string& initial_planner, std::string& error);
|
||||
|
||||
bool configured() const
|
||||
{
|
||||
return configured_;
|
||||
}
|
||||
|
||||
// ================================================================================================
|
||||
// PlannerPort
|
||||
// ================================================================================================
|
||||
|
||||
bool swapPlanner(const std::string& planner_name) override;
|
||||
|
||||
bool startPlan(const robot_geometry_msgs::PoseStamped& start,
|
||||
const robot_geometry_msgs::PoseStamped& goal,
|
||||
const robot_protocol_msgs::Order* order, std::uint64_t tag) override;
|
||||
|
||||
bool isPlanning() const override;
|
||||
|
||||
bool pollPlan(PlanResult& result) override;
|
||||
|
||||
void cancelPlan() override;
|
||||
|
||||
std::string activePlanner() const override;
|
||||
|
||||
/// @brief Số plugin đã nạp và còn giữ trong cache — để kiểm việc dùng lại, không phải để log.
|
||||
std::size_t loadedCount() const
|
||||
{
|
||||
return planners_.size();
|
||||
}
|
||||
|
||||
private:
|
||||
/// @brief Một plugin đã nạp: factory phải sống cùng instance, xem doc của lớp.
|
||||
struct Loaded
|
||||
{
|
||||
std::function<robot_nav_core::BaseGlobalPlanner::Ptr()> factory;
|
||||
robot_nav_core::BaseGlobalPlanner::Ptr instance;
|
||||
};
|
||||
|
||||
/// @brief Nạp @p name nếu chưa có trong cache. @return nullptr khi thất bại (đã log lý do).
|
||||
robot_nav_core::BaseGlobalPlanner* acquire(const std::string& name);
|
||||
|
||||
/// @brief Thân thread: ngủ tới khi có yêu cầu, chạy plugin, đặt kết quả vào hộp thư.
|
||||
void threadBody();
|
||||
|
||||
// --- Chỉ control thread chạm ------------------------------------------------------------------
|
||||
robot::NodeHandle nh_;
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_ = nullptr;
|
||||
bool configured_ = false;
|
||||
std::map<std::string, Loaded> planners_;
|
||||
std::string active_name_;
|
||||
|
||||
// --- Chia sẻ giữa hai thread, bảo vệ bởi mutex_ ------------------------------------------------
|
||||
mutable std::mutex mutex_;
|
||||
std::condition_variable cv_;
|
||||
std::thread thread_;
|
||||
|
||||
bool shutdown_ = false;
|
||||
bool pending_ = false; ///< Có yêu cầu chờ thread nhận.
|
||||
bool running_ = false; ///< Thread đang chạy plugin.
|
||||
bool discard_ = false; ///< Lượt đang chạy đã bị huỷ — vứt kết quả khi nó về.
|
||||
bool has_result_ = false;
|
||||
|
||||
robot_nav_core::BaseGlobalPlanner* active_ = nullptr; ///< Non-owning, trỏ vào @ref planners_.
|
||||
robot_geometry_msgs::PoseStamped request_start_;
|
||||
robot_geometry_msgs::PoseStamped request_goal_;
|
||||
std::shared_ptr<robot_protocol_msgs::Order> request_order_;
|
||||
std::uint64_t request_tag_ = 0;
|
||||
|
||||
std::vector<robot_geometry_msgs::PoseStamped> planning_; ///< Thread ghi vào.
|
||||
std::vector<robot_geometry_msgs::PoseStamped> handoff_; ///< Hộp thư.
|
||||
std::uint64_t result_tag_ = 0;
|
||||
bool result_ok_ = false;
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_RUNNERS_PLANNER_RUNNER_H_
|
||||
164
include/move_base2/runners/recovery_runner.h
Normal file
164
include/move_base2/runners/recovery_runner.h
Normal file
@@ -0,0 +1,164 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* move_base2 — hiện thực RecoveryPort bằng recovery_core.
|
||||
*
|
||||
* Author: DuongTD
|
||||
*********************************************************************/
|
||||
#ifndef MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_
|
||||
#define MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_
|
||||
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <recovery_core/adapters/costmap_collision_checker.h>
|
||||
#include <recovery_core/recovery_registry.h>
|
||||
|
||||
#include <move_base2/ports/clock_port.h>
|
||||
#include <move_base2/ports/pose_port.h>
|
||||
#include <move_base2/ports/recovery_port.h>
|
||||
|
||||
namespace robot_costmap_2d { class Costmap2DROBOT; }
|
||||
|
||||
namespace move_base2
|
||||
{
|
||||
|
||||
/**
|
||||
* @class RecoveryRunner
|
||||
* @brief Nối @ref RecoveryPort của lõi với framework `recovery_core`.
|
||||
*
|
||||
* Đây là **file duy nhất** trong `move_base2` include `recovery_core`. Nhờ vậy hai bên nằm trong
|
||||
* cùng một translation unit và compiler kiểm được toàn bộ contract, dù plugin vẫn nạp qua
|
||||
* `boost::dll` lúc chạy.
|
||||
*
|
||||
* Nó cũng là chỗ dịch **ba cặp khái niệm song song** giữa hai gói — dịch ở đúng một nơi:
|
||||
*
|
||||
* | `move_base2` | `recovery_core` |
|
||||
* |---|---|
|
||||
* | `PosePort` | `PoseProvider` (adapter nội bộ bên dưới) |
|
||||
* | `ClockPort` | tham số `robot::Time` của `update(now)` |
|
||||
* | `RecoveryTrigger` | `RecoveryTrigger` (`switch` 1:1) |
|
||||
*
|
||||
* Trùng khái niệm là **cố ý**: chiều phụ thuộc một chiều (D3) cấm `recovery_core` biết tới
|
||||
* `move_base2`. Đừng sinh phiên bản thứ ba của cùng khái niệm ở nơi khác.
|
||||
*
|
||||
* @note Không thread-safe. Chỉ control thread được gọi — thread duy nhất phát cmd_vel.
|
||||
*/
|
||||
class RecoveryRunner final : public RecoveryPort
|
||||
{
|
||||
public:
|
||||
/// @brief Nguồn plan hiện hành, cấp cho behavior họ path. Trả false nếu chưa có plan.
|
||||
using PlanSource = std::function<bool(std::vector<robot_geometry_msgs::PoseStamped>&)>;
|
||||
|
||||
/**
|
||||
* @struct Deps
|
||||
* @brief Các cổng runtime mà runner cần. Tất cả **non-owning**.
|
||||
*
|
||||
* Con trỏ costmap có thể bị thay giữa hai lượt — dùng @ref setCostmaps để cập nhật thay vì để
|
||||
* runner giữ một bản cache từ lúc configure.
|
||||
*/
|
||||
struct Deps
|
||||
{
|
||||
ClockPort* clock = nullptr;
|
||||
PosePort* pose = nullptr;
|
||||
robot_costmap_2d::Costmap2DROBOT* local_costmap = nullptr;
|
||||
robot_costmap_2d::Costmap2DROBOT* global_costmap = nullptr;
|
||||
};
|
||||
|
||||
RecoveryRunner() = default;
|
||||
|
||||
/// @brief Nạp cổng. Gọi trước @ref configure.
|
||||
void setDeps(const Deps& deps);
|
||||
|
||||
/// @brief Cập nhật con trỏ costmap khi chúng bị thay. Non-owning.
|
||||
void setCostmaps(robot_costmap_2d::Costmap2DROBOT* local,
|
||||
robot_costmap_2d::Costmap2DROBOT* global);
|
||||
|
||||
/// @brief Namespace YAML chứa `<ns>/behaviors`. Mặc định "recovery".
|
||||
void setNamespace(const std::string& ns);
|
||||
|
||||
/// @brief Nguồn plan cho behavior họ path. Không đặt = không có plan.
|
||||
void setPlanSource(PlanSource source);
|
||||
|
||||
/**
|
||||
* @copydoc RecoveryPort::configure
|
||||
*
|
||||
* Nạp danh sách behavior qua `recovery_core::RecoveryRegistry`. Behavior nào `configure()` hỏng
|
||||
* thì bị bỏ và log đích danh — trả `false`, nhưng các behavior còn lại **vẫn** dùng được: một
|
||||
* đường phục hồi hỏng không nên xoá sạch các đường còn lại.
|
||||
*/
|
||||
bool configure(robot::NodeHandle& nh) override;
|
||||
|
||||
std::size_t behaviorCount() const override;
|
||||
RecoveryOutputKind outputKind(std::size_t index) const override;
|
||||
bool start(std::size_t index, RecoveryTrigger trigger) override;
|
||||
RecoveryTick update() override;
|
||||
void cancel() override;
|
||||
std::string behaviorName(std::size_t index) const override;
|
||||
|
||||
private:
|
||||
/// Chuyển `PosePort` của lõi thành cổng pose của recovery_core.
|
||||
class PoseBridge final : public recovery_core::PoseProvider
|
||||
{
|
||||
public:
|
||||
explicit PoseBridge(const PosePort* port = nullptr) : port_(port)
|
||||
{
|
||||
}
|
||||
|
||||
void setPort(const PosePort* port)
|
||||
{
|
||||
port_ = port;
|
||||
}
|
||||
|
||||
bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
|
||||
{
|
||||
return port_ != nullptr && port_->getRobotPose(pose);
|
||||
}
|
||||
|
||||
private:
|
||||
const PosePort* port_ = nullptr; ///< non-owning
|
||||
};
|
||||
|
||||
/// Chuyển callback plan của runtime thành cổng plan của recovery_core.
|
||||
class PlanBridge final : public recovery_core::PlanProvider
|
||||
{
|
||||
public:
|
||||
void setSource(PlanSource source)
|
||||
{
|
||||
source_ = std::move(source);
|
||||
}
|
||||
|
||||
bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const override
|
||||
{
|
||||
return source_ ? source_(out) : false;
|
||||
}
|
||||
|
||||
private:
|
||||
PlanSource source_;
|
||||
};
|
||||
|
||||
/// Trỏ lại context vào con trỏ costmap hiện hành trước mỗi lượt dùng.
|
||||
void refreshContext();
|
||||
|
||||
/// Dịch kết quả của recovery_core sang ngôn ngữ của lõi.
|
||||
RecoveryTick toTick(const recovery_core::RecoveryResult& result) const;
|
||||
|
||||
Deps deps_;
|
||||
std::string namespace_ = "recovery";
|
||||
|
||||
recovery_core::RecoveryRegistry registry_;
|
||||
recovery_core::RecoveryContext ctx_;
|
||||
recovery_core::CostmapCollisionChecker collision_;
|
||||
PoseBridge pose_bridge_;
|
||||
PlanBridge plan_bridge_;
|
||||
|
||||
bool configured_ = false;
|
||||
recovery_core::RecoveryBehavior* active_ = nullptr; ///< non-owning, thuộc registry_
|
||||
};
|
||||
|
||||
} // namespace move_base2
|
||||
|
||||
#endif // MOVE_BASE2_RUNNERS_RECOVERY_RUNNER_H_
|
||||
Reference in New Issue
Block a user