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

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:40:26 +07:00
parent 33ee9b7f31
commit 2223453639
120 changed files with 12204 additions and 1886 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
include/mission_adapters/adapter.h
View File

@@ -0,0 +1,95 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Interface plugin duy nhất của gói: nguồn mission.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_ADAPTER_H_
#define MISSION_ADAPTERS_ADAPTER_H_
#include <memory>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <mission_adapters/mission_request.h>
#include <mission_adapters/types.h>
namespace mission_adapters
{
/// @brief Quan hệ giữa mission mới và hàng đợi đang có.
enum class SubmitMode
{
/// Yêu cầu mới: thay toàn bộ hàng đợi, chặng đang chạy bị preempt (và được bảo dừng).
kReplace,
/**
* Phần nối tiếp của yêu cầu đang chạy: chỉ thêm vào cuối hàng đợi.
*
* Đây là hình dạng của VDA5050 order update — phần horizon được release thêm. Xử lý nó như
* kReplace sẽ huỷ và chạy lại chặng đang đi, dù tuyến đường không hề đổi.
*/
kAppend
};
/// @brief Kết quả chuyển đổi: mission sinh ra và cách chúng vào hàng đợi.
struct ConversionResult
{
std::vector<std::shared_ptr<Mission>> missions;
SubmitMode mode = SubmitMode::kReplace;
bool empty() const { return missions.empty(); }
};
/**
* @class MissionSourceAdapter
* @brief Biến payload của một nguồn cụ thể thành mission của mission layer.
*
* Plugin được nạp bằng Boost.DLL theo khoá `library_path` trong YAML, giống mọi plugin khác của
* workspace. Contract này là public interface qua ranh giới .so: đổi chữ ký thì phải đổi cả
* registry trong cùng một lần sửa, vì import_alias là dlsym + reinterpret_cast và không có kiểm
* kiểu nào bắt được sự lệch đó lúc biên dịch.
*
* Việc thực thi action KHÔNG thuộc về interface này: adapter chỉ chuyển dữ liệu, navigation
* runtime mới là nơi chạy action (D8).
*/
class MissionSourceAdapter
{
public:
using Ptr = std::shared_ptr<MissionSourceAdapter>;
virtual ~MissionSourceAdapter() = default;
/**
* @brief Đọc param riêng của instance. Gọi đúng một lần khi nạp.
* @param name Tên instance trong `mission_sources` — cũng là namespace param của nó.
* @param nh NodeHandle gốc.
* @return false nếu cấu hình không hợp lệ; registry sẽ báo lỗi và không đăng ký adapter này.
*/
virtual bool configure(const std::string& name, robot::NodeHandle& nh) = 0;
/// @brief Schema mà adapter này xử lý. Core định tuyến theo đúng giá trị này.
virtual std::string schema() const = 0;
/**
* @brief Kiểm payload TRƯỚC khi convert.
* @param[out] reason Lý do từ chối, dùng cho log.
* @return false -> core từ chối sớm và không đụng vào hàng đợi.
*/
virtual bool validate(const MissionRequest& request, std::string& reason) const = 0;
/**
* @brief Chuyển payload thành 0..n mission, kèm cách chúng vào hàng đợi.
*
* Trả rỗng nghĩa là **không có việc**, KHÔNG phải lỗi và cũng không phải lệnh huỷ: core sẽ
* bỏ qua, giữ nguyên hàng đợi đang chạy (A1).
*/
virtual ConversionResult convert(const MissionRequest& request) = 0;
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_ADAPTER_H_

121
include/mission_adapters/event.h
View File

@@ -0,0 +1,121 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Sự kiện của mission layer và hàng đợi vận chuyển chúng.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_EVENT_H_
#define MISSION_ADAPTERS_EVENT_H_
#include <atomic>
#include <condition_variable>
#include <cstdint>
#include <memory>
#include <mutex>
#include <queue>
#include <vector>
#include <mission_adapters/mission_request.h>
#include <mission_adapters/types.h>
namespace mission_adapters
{
enum class EventType
{
SUBMIT_REQUEST,
NAV_DONE,
NAV_FAILED,
PAUSE,
RESUME,
CANCEL,
EMERGENCY,
CLEAR_EMERGENCY
};
struct Event
{
EventType type = EventType::SUBMIT_REQUEST;
uint64_t sequence = 0; ///< thứ tự phát sinh, EventBus tự gán
/// Chỉ dùng cho NAV_DONE / NAV_FAILED: mission nào vừa kết thúc.
MissionId mission_id = kInvalidMissionId;
/// Chỉ dùng cho SUBMIT_REQUEST: payload chưa chuyển đổi, adapter sẽ xử lý trên thread worker.
MissionRequest request;
};
/**
* @class EventBus
* @brief Hàng đợi sự kiện FIFO, cộng một đường out-of-band riêng cho emergency.
*
* **FIFO là bắt buộc, không phải lựa chọn phong cách.** Bản trước sắp xếp theo priority toàn
* phần nên CANCEL (ưu tiên cao) luôn được xử lý trước SUBMIT (ưu tiên thấp) — phát order rồi huỷ
* ngay thì huỷ chạy trước, order vào hàng đợi sau và robot chạy đúng cái người dùng vừa huỷ.
* Thứ tự xử lý phải bằng thứ tự phát sinh thì quan hệ nhân quả mới còn.
*
* Emergency không đi theo luật đó: nó bật một cờ atomic ngay tại chỗ gọi, nên độ trễ phản ứng
* không phụ thuộc vào việc hàng đợi đang dài bao nhiêu. Sự kiện EMERGENCY vẫn được đẩy vào hàng
* đợi để dòng thời gian trong log không bị thủng.
*/
class EventBus
{
public:
/// @brief Đẩy một sự kiện thường vào cuối hàng đợi.
void push(Event event);
/**
* @brief Đẩy sự kiện emergency: bật cờ out-of-band trước, rồi mới xếp hàng.
*
* Bên tiêu thụ phải gọi @ref takeEmergency trước khi xử lý sự kiện tiếp theo.
*/
void pushEmergency(Event event);
/// @brief Lấy sự kiện kế tiếp, chờ nếu hàng đợi rỗng. Trả false khi bus đã stop().
bool pop(Event& event);
/**
* @brief Lấy và xoá cờ emergency đang treo.
* @return true nếu có emergency chưa được xử lý.
*/
bool takeEmergency();
/// @brief Có emergency đang treo hay không (không xoá cờ).
bool emergencyPending() const;
void stop();
void reset();
bool empty() const
{
std::lock_guard<std::mutex> lock(mutex_);
return queue_.empty();
}
size_t size() const
{
std::lock_guard<std::mutex> lock(mutex_);
return queue_.size();
}
void clear()
{
std::lock_guard<std::mutex> lock(mutex_);
queue_ = {};
}
private:
mutable std::mutex mutex_;
std::queue<Event> queue_;
std::condition_variable cv_;
bool stop_ = false;
uint64_t next_sequence_ = 0;
/// Ngoài hàng đợi có chủ đích: emergency không được xếp sau một hàng dài sự kiện thường.
std::atomic<bool> emergency_pending_{false};
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_EVENT_H_

93
include/mission_adapters/event_processor.h
View File

@@ -0,0 +1,93 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Thread tiêu thụ event: biến sự kiện từ host thành lời gọi lên MissionManager.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_EVENT_PROCESSOR_H_
#define MISSION_ADAPTERS_EVENT_PROCESSOR_H_
#include <atomic>
#include <thread>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_protocol_msgs/Order.h>
#include <mission_adapters/event.h>
#include <mission_adapters/mission_manager.h>
#include <mission_adapters/mission_request.h>
#include <mission_adapters/plugin_registry.h>
namespace mission_adapters
{
/**
* @class EventProcessor
* @brief Thread duy nhất biến sự kiện thành thay đổi trạng thái mission.
*
* Core không biết adapter nào tồn tại: nó chỉ tra @ref PluginRegistry theo
* MissionRequest::schema. Thêm một loại nguồn mới = thêm plugin + một dòng YAML, không sửa file
* nào trong `src/` của gói.
*
* Việc chuyển payload -> mission chạy trên đúng thread này (không phải thread của host) vì
* adapter được phép có state; chuyển đổi trên nhiều thread host sẽ tranh chấp state đó.
*/
class EventProcessor
{
public:
EventProcessor(MissionManager& mission_manager, PluginRegistry& registry);
~EventProcessor();
void start();
void stop();
/**
* @brief Đường vào chung cho mọi nguồn mission.
*
* Nguồn mới chỉ cần dựng MissionRequest với schema của mình rồi gọi hàm này.
*/
void submitRequest(const MissionRequest& request);
/// @brief Tiện ích cho nguồn goal đơn lẻ. Chỉ dựng envelope, không biết adapter nào xử lý.
void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
/// @brief Tiện ích cho nguồn VDA5050.
void orderEvent(const robot_protocol_msgs::Order& order);
void cancelEvent();
void pauseEvent();
void resumeEvent();
/**
* @brief Báo một chặng đã hoàn tất (nav + action — D8).
* @param id Mission vừa xong. Bắt buộc: outcome không mang ID sẽ bị manager loại bỏ.
*/
void navDoneEvent(MissionId id);
/// @brief Báo một chặng thất bại. Cũng bắt buộc mang MissionId.
void navFailedEvent(MissionId id);
void emergencyEvent();
void clearEmergencyEvent();
private:
void spin();
void process(const Event& event);
/// Tra adapter theo schema, validate, convert, rồi submit nếu có mission.
void handleRequest(const MissionRequest& request);
/// Xử lý emergency đang treo (nếu có) trước khi động tới hàng đợi sự kiện thường.
void handlePendingEmergency();
EventBus event_bus_;
MissionManager& mission_manager_;
PluginRegistry& registry_;
std::thread worker_;
std::atomic<bool> running_{false};
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_EVENT_PROCESSOR_H_

256
include/mission_adapters/mission_adapters.h
View File

@@ -1,245 +1,25 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Header façade — gộp toàn bộ public API của gói.
*
* Giữ lại để code đã include header này không phải sửa. Code mới nên include thẳng header cần dùng
* (types.h / event.h / adapter.h / mission_manager.h / event_processor.h / mission_executor.h).
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H
#define MISSION_ADAPTERS_MISSION_ADAPTERS_H
#include <string>
#include <vector>
#include <queue>
#include <memory>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <functional>
#include <atomic>
#include <cstdint>
#include <robot/robot.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_geometry_msgs/PoseStamped.h>
namespace mission_adapters
{
// ── Priority constants ────────────────────────────────────────────────────
// Số càng nhỏ = ưu tiên càng cao (min-heap)
static constexpr int PRIORITY_EMERGENCY = 0;
static constexpr int PRIORITY_CANCEL = 1;
static constexpr int PRIORITY_RESUME = 2;
static constexpr int PRIORITY_PAUSE = 3;
static constexpr int PRIORITY_NAV_DONE = 4;
// FIX #4: Dedicated constant — same value as NAV_DONE but semantically correct name.
static constexpr int PRIORITY_NAV_FAILED = 4;
static constexpr int PRIORITY_ORDER = 5;
enum class MissionState
{
IDLE,
QUEUED,
RUNNING,
PAUSED,
RECOVERY,
COMPLETED,
FAILED,
CANCELLED,
EMERGENCY,
CLEAR_EMERGENCY
};
enum class EventType
{
SUBMIT_MISSIONS,
NAV_DONE,
NAV_FAILED,
PAUSE,
RESUME,
CANCEL,
EMERGENCY,
CLEAR_EMERGENCY
};
enum class MissionType
{
SIMPLE_GOAL,
VDA5050_ORDER
};
enum class ActionType
{
NODE_ACTION,
EDGE_ACTION
};
class Action
{
public:
int sequenceId = 0;
ActionType type = ActionType::NODE_ACTION;
robot_protocol_msgs::Action action;
};
class Mission
{
public:
int sequenceId = 0;
MissionType type = MissionType::SIMPLE_GOAL;
int priority = 0;
robot_geometry_msgs::PoseStamped start;
robot_geometry_msgs::PoseStamped goal;
std::vector<robot_protocol_msgs::Node> nodes;
std::vector<robot_protocol_msgs::Edge> edges;
std::vector<Action> actions;
};
struct Event
{
EventType type = EventType::SUBMIT_MISSIONS;
int priority = PRIORITY_ORDER;
uint64_t sequence = 0;
std::vector<std::shared_ptr<Mission>> missions;
};
struct EventCompare
{
bool operator()(const Event& lhs, const Event& rhs) const
{
if (lhs.priority != rhs.priority)
return lhs.priority > rhs.priority;
return lhs.sequence > rhs.sequence;
}
};
class EventBus
{
private:
mutable std::mutex mutex_;
std::priority_queue<Event, std::vector<Event>, EventCompare> queue_;
std::condition_variable cv_;
bool stop_ = false;
uint64_t next_sequence_ = 0;
public:
void push(const Event& event);
void push(Event&& event);
bool pop(Event& event);
void stop();
void reset();
inline bool empty() const
{
std::lock_guard<std::mutex> lock(mutex_);
return queue_.empty();
}
inline void clear()
{
std::lock_guard<std::mutex> lock(mutex_);
while (!queue_.empty()) queue_.pop();
}
};
class VDA5050Adapter
{
public:
std::vector<std::shared_ptr<Mission>> convert(const robot_protocol_msgs::Order& order);
};
class GoalAdapter
{
public:
std::vector<std::shared_ptr<Mission>> convert(const robot_geometry_msgs::PoseStamped& goal);
};
class MissionManager
{
public:
void submit(const std::vector<std::shared_ptr<Mission>>& missions);
std::shared_ptr<Mission> nextMission();
void onNavigationDone();
void onNavigationFailed();
void cancel();
void pause();
void resume();
void emergency();
void clearEmergency();
MissionState state() const;
// FIX #3: Returns true if there is any pending work — queue OR active mission.
bool hasMission() const;
private:
mutable std::mutex mutex_;
MissionState state_ = MissionState::IDLE;
std::queue<std::shared_ptr<Mission>> mission_queue_;
std::shared_ptr<Mission> current_mission_;
};
class EventProcessor
{
public:
explicit EventProcessor(MissionManager& mission_manager);
~EventProcessor();
void start();
void stop();
void goalEvent(const robot_geometry_msgs::PoseStamped& goal);
void orderEvent(const robot_protocol_msgs::Order& order);
void cancelEvent();
void pauseEvent();
void resumeEvent();
void navDoneEvent();
void navFailedEvent();
void emergencyEvent();
void clearEmergencyEvent();
private:
void spin();
void process(const Event& event);
private:
EventBus event_bus_;
GoalAdapter goal_adapter_;
VDA5050Adapter vda5050_adapter_;
MissionManager& mission_manager_;
std::thread worker_;
std::atomic<bool> running_{false};
};
class MissionExecutor
{
public:
using MissionCallback = std::function<void(const std::shared_ptr<Mission>&)>;
explicit MissionExecutor(MissionManager& manager);
~MissionExecutor();
void start();
void stop();
// FIX #1: Must be called BEFORE start(), not inside the run loop.
void setMissionCallback(MissionCallback cb)
{
std::lock_guard<std::mutex> lock(mutex_);
mission_callback_ = std::move(cb);
}
private:
std::shared_ptr<Mission> mission_execute_;
// Last mission that was dispatched to the callback.
std::shared_ptr<Mission> last_dispatched_mission_;
MissionCallback mission_callback_;
MissionManager& mission_manager_;
mutable std::mutex mutex_;
std::thread worker_;
std::atomic<bool> running_{false};
void spin();
};
} // namespace mission_adapters
#include <mission_adapters/adapter.h>
#include <mission_adapters/event.h>
#include <mission_adapters/event_processor.h>
#include <mission_adapters/mission_executor.h>
#include <mission_adapters/mission_manager.h>
#include <mission_adapters/navigation_client.h>
#include <mission_adapters/types.h>
#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H

64
include/mission_adapters/mission_config.h Normal file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Tham số vận hành của mission layer.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_CONFIG_H_
#define MISSION_ADAPTERS_MISSION_CONFIG_H_
#include <string>
#include <robot/node_handle.h>
namespace mission_adapters
{
/**
* @struct MissionConfig
* @brief Tham số vận hành, nạp từ `mission_adapters_params.yaml`.
*
* Bản runtime của file nằm ở `pnkx_nav_core/config/`; bản trong `test/config/` chỉ dùng cho test
* và phải chạy kèm PNKX_NAV_CORE_CONFIG_DIR trỏ vào đó.
*/
struct MissionConfig
{
/**
* Trần thời gian cho MỘT chặng (nav + action), tính từ lúc chặng được giao. [s]
*
* 0 = tắt. Quá hạn thì chặng bị đánh dấu thất bại và navigation được bảo dừng — lưới cuối
* cho trường hợp navigation không bao giờ báo kết quả về.
*/
double mission_timeout = 0.0;
/**
* Một chặng thất bại thì xử lý phần còn lại của hàng đợi thế nào.
*
* `true` (mặc định) = xoá sạch hàng đợi. Đây là hành vi an toàn cho tuyến đường tuần tự
* kiểu VDA5050: chặng 2 hỏng nghĩa là robot không tới được node 2, nên chạy tiếp chặng 3 là
* cắt ngang qua đoạn đường mà fleet manager chưa cho phép đi.
*
* `false` = chỉ bỏ chặng lỗi rồi chạy tiếp chặng sau. Chỉ đặt false khi các mission trong
* hàng đợi ĐỘC LẬP với nhau (nhiều goal rời rạc), không phải các chặng của cùng một tuyến.
*/
bool clear_queue_on_failure = true;
/**
* @brief Nạp tham số từ namespace @p ns.
* @return false nếu giá trị nạp được không qua @ref validate.
*
* Thiếu khoá không phải lỗi: giá trị mặc định trong struct này là hợp lệ và an toàn.
*/
bool loadFromParams(robot::NodeHandle& nh, const std::string& ns = "mission_adapters");
/// @brief Kiểm miền giá trị. Trả false kèm log nêu đích danh tham số sai.
bool validate() const;
/// @brief In cấu hình đang dùng — để log khởi động nói được vì sao robot hành xử như vậy.
void print() const;
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_MISSION_CONFIG_H_

0
include/mission_adapters/mission_excutor.h
View File

62
include/mission_adapters/mission_executor.h Normal file
View File

@@ -0,0 +1,62 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Thread lấy mission kế tiếp từ MissionManager và đẩy xuống phía navigation.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_EXECUTOR_H_
#define MISSION_ADAPTERS_MISSION_EXECUTOR_H_
#include <atomic>
#include <memory>
#include <mutex>
#include <thread>
#include <mission_adapters/mission_manager.h>
#include <mission_adapters/navigation_client.h>
#include <mission_adapters/types.h>
namespace mission_adapters
{
/**
* @class MissionExecutor
* @brief Thread duy nhất phát lệnh xuống navigation.
*
* Mọi lời gọi ra ngoài (dispatch, cancelActive) đi ra từ đây, nên MissionManager không bao giờ
* gọi ra ngoài khi đang giữ khoá. Thứ tự trong một vòng là bất biến an toàn: **cancel trước,
* dispatch sau** — chặng cũ phải được bảo dừng trước khi chặng mới bắt đầu.
*/
class MissionExecutor
{
public:
explicit MissionExecutor(MissionManager& manager);
~MissionExecutor();
/**
* @brief Gắn cổng navigation. Con trỏ non-owning, phải sống lâu hơn executor.
*
* FIX #1: gọi TRƯỚC start(), không gọi trong vòng lặp.
*/
void setNavigationClient(NavigationClient* client);
void start();
void stop();
private:
void spin();
/// Một vòng công việc. Tách riêng để test gọi trực tiếp, không cần thread.
void step();
NavigationClient* navigation_client_ = nullptr; ///< non-owning, có thể null
MissionManager& mission_manager_;
mutable std::mutex mutex_;
std::thread worker_;
std::atomic<bool> running_{false};
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_MISSION_EXECUTOR_H_

165
include/mission_adapters/mission_manager.h
View File

@@ -0,0 +1,165 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Trạng thái và hàng đợi mission — nơi duy nhất giữ MissionState.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_MANAGER_H_
#define MISSION_ADAPTERS_MISSION_MANAGER_H_
#include <condition_variable>
#include <memory>
#include <mutex>
#include <queue>
#include <vector>
#include <robot/time.h>
#include <mission_adapters/mission_config.h>
#include <mission_adapters/types.h>
namespace mission_adapters
{
/**
* @class MissionManager
* @brief Hàng đợi mission và trạng thái của nó. Thread-safe, không tự gọi ra ngoài.
*
* Manager cố ý KHÔNG giữ con trỏ tới navigation: nó chỉ ghi nhận "mission này cần được dừng"
* (@ref takePendingCancel) và để MissionExecutor thực hiện lời gọi đó. Nhờ vậy không có lời gọi
* ra ngoài nào xảy ra khi đang giữ khoá, và mọi lệnh xuống navigation đi ra từ đúng một thread.
*/
class MissionManager
{
public:
MissionManager() = default;
explicit MissionManager(const MissionConfig& config) : config_(config) {}
/// @brief Đổi tham số vận hành. Gọi trước khi có mission nào đang chạy.
void setConfig(const MissionConfig& config);
/**
* @brief Nhận một loạt mission mới và cấp @ref MissionId cho từng cái.
*
* Semantics (A1): danh sách rỗng là **no-op tuyệt đối** — không đụng hàng đợi, không đổi
* trạng thái. Một order lỗi không phải là lệnh huỷ việc đang chạy.
*
* Submit khi đang RUNNING là **preempt tường minh**: mission đang chạy được ghi vào ô
* pending-cancel để navigation được bảo dừng, rồi hàng đợi mới thay hàng đợi cũ.
*/
void submit(const std::vector<std::shared_ptr<Mission>>& missions);
/**
* @brief Nối thêm mission vào cuối hàng đợi, KHÔNG đụng chặng đang chạy.
*
* Dùng cho phần nối tiếp của một yêu cầu đang chạy (VDA5050 order update release thêm
* horizon). Khác @ref submit ở chỗ không preempt và không yêu cầu dừng navigation — tuyến
* đường không đổi thì robot không có lý do gì phải dừng lại.
*/
void append(const std::vector<std::shared_ptr<Mission>>& missions);
/**
* @brief Lấy mission kế tiếp cần chạy.
* @return Mission MỚI, hoặc nullptr nếu chưa tới lượt.
*
* Chỉ trả khác null đúng một lần cho mỗi mission, tại đúng bước chuyển QUEUED -> RUNNING
* (M6). Bên gọi vì thế không cần tự khử trùng lặp.
*/
std::shared_ptr<const Mission> nextMission();
/**
* @brief Ghi nhận một chặng hoàn tất (cả navigation lẫn action — D8).
* @param id Mission vừa xong.
* @return false nếu outcome không thuộc mission đang chạy (đến trễ sau khi bị thay thế).
*/
bool onNavigationDone(MissionId id);
/**
* @brief Ghi nhận một chặng thất bại.
* @return false nếu outcome không thuộc mission đang chạy.
*/
bool onNavigationFailed(MissionId id);
void cancel();
void pause();
void resume();
void emergency();
void clearEmergency();
MissionState state() const;
/// @brief Có việc đang treo hay không — hàng đợi HOẶC mission đang chạy.
bool hasMission() const;
/// @brief Mission đang chạy, kInvalidMissionId nếu không có.
MissionId currentMissionId() const;
/**
* @brief Lấy và xoá yêu cầu dừng navigation đang treo.
* @return kInvalidMissionId nếu không có yêu cầu nào.
*
* Chỉ MissionExecutor gọi hàm này; nó là cách manager nhờ executor gọi
* NavigationClient::cancelActive() mà bản thân không cần biết navigation là gì.
*/
MissionId takePendingCancel();
/**
* @brief Chờ tới khi có việc cho executor: mission mới cần giao, hoặc yêu cầu dừng đang treo.
* @return false nếu bị đánh thức bởi @ref wakeUp (đang dừng), true nếu thật sự có việc.
*
* Thay cho vòng poll: mission layer không có deadline định kỳ nào, poll chỉ thêm độ trễ
* trung bình nửa chu kỳ vào đường phản ứng cancel/emergency.
*/
bool waitForWork();
/// @brief Đánh thức mọi thread đang chờ trong @ref waitForWork (dùng khi dừng executor).
void wakeUp();
private:
/// Gọi khi đã giữ mutex_. Ghi mission đang chạy vào ô pending-cancel rồi bỏ nó ra.
void requestCancelOfCurrentLocked();
/// Gọi khi đã giữ mutex_. Id mission đang chạy, kInvalidMissionId nếu không có.
MissionId currentIdLocked() const;
/**
* Gọi khi đã giữ mutex_. Đổi trạng thái và ghi log ĐÚNG MỘT LẦN tại mỗi lần đổi thật.
*
* Đây là nơi duy nhất được gán state_: nếu không, mission biến mất khỏi hàng đợi mà không
* có dòng log nào cho biết ai đã làm điều đó và vì sao.
*
* @param reason Lý do ngắn gọn, cho người đọc log lúc 2 giờ sáng.
*/
void setStateLocked(MissionState next, const char* reason, MissionId id = kInvalidMissionId);
/// Gọi khi đã giữ mutex_. true nếu executor có việc để làm ngay.
bool hasWorkLocked() const;
/// Gọi khi đã giữ mutex_. true nếu đang có chặng chạy và mission_timeout được bật.
bool timeoutArmedLocked() const;
/**
* Gọi khi đã giữ mutex_. Đánh dấu chặng đang chạy là thất bại do quá hạn, và ghi yêu cầu
* dừng navigation — khác NAV_FAILED ở chỗ navigation vẫn đang chạy nên phải bảo nó dừng.
*/
void expireCurrentLocked();
mutable std::mutex mutex_;
std::condition_variable work_cv_;
bool wake_requested_ = false;
MissionState state_ = MissionState::IDLE;
std::queue<std::shared_ptr<const Mission>> mission_queue_;
std::shared_ptr<const Mission> current_mission_;
MissionId pending_cancel_ = kInvalidMissionId;
/// Thời điểm chặng hiện tại được giao — mốc tính mission_timeout.
robot::Time mission_start_time_;
MissionConfig config_;
MissionId next_mission_id_ = 1; ///< 0 dành riêng cho kInvalidMissionId
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_MISSION_MANAGER_H_

77
include/mission_adapters/mission_request.h Normal file
View File

@@ -0,0 +1,77 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Envelope cho payload đầu vào của mission layer.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_MISSION_REQUEST_H_
#define MISSION_ADAPTERS_MISSION_REQUEST_H_
#include <memory>
#include <string>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_protocol_msgs/Order.h>
namespace mission_adapters
{
/// @brief Tên schema của các nguồn có sẵn. Nguồn mới tự khai schema riêng, không cần sửa ở đây.
namespace schema
{
constexpr const char* kVda5050Order = "vda5050.order";
constexpr const char* kPoseStamped = "geometry.pose_stamped";
} // namespace schema
/**
* @struct MissionRequest
* @brief Payload đầu vào cùng với tên schema mô tả nó.
*
* Hai nguồn hiện có nhận hai kiểu khác hẳn nhau (Order và PoseStamped) nên không thể có một
* convert() chung nếu không bọc lại. Core chỉ định tuyến theo @ref schema và không bao giờ cần
* biết kiểu thật; thêm nguồn mới = thêm schema mới + plugin mới.
*
* Payload có kiểu dùng cho nguồn nội bộ (tránh serialize thừa); nguồn ngoài (REST/MQTT/file) đi
* bằng @ref raw_payload và tự parse trong plugin của mình.
*/
struct MissionRequest
{
std::string schema;
std::shared_ptr<robot_protocol_msgs::Order> order;
std::shared_ptr<robot_geometry_msgs::PoseStamped> pose;
std::string raw_payload;
/// @brief Dựng request từ một VDA5050 Order.
static MissionRequest fromOrder(const robot_protocol_msgs::Order& order)
{
MissionRequest request;
request.schema = schema::kVda5050Order;
request.order = std::make_shared<robot_protocol_msgs::Order>(order);
return request;
}
/// @brief Dựng request từ một goal đơn lẻ.
static MissionRequest fromPose(const robot_geometry_msgs::PoseStamped& pose)
{
MissionRequest request;
request.schema = schema::kPoseStamped;
request.pose = std::make_shared<robot_geometry_msgs::PoseStamped>(pose);
return request;
}
/// @brief Dựng request từ payload thô của một nguồn ngoài.
static MissionRequest fromRaw(const std::string& schema_name, const std::string& payload)
{
MissionRequest request;
request.schema = schema_name;
request.raw_payload = payload;
return request;
}
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_MISSION_REQUEST_H_

55
include/mission_adapters/navigation_client.h Normal file
View File

@@ -0,0 +1,55 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Cổng ra phía navigation của mission layer.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_NAVIGATION_CLIENT_H_
#define MISSION_ADAPTERS_NAVIGATION_CLIENT_H_
#include <memory>
#include <mission_adapters/types.h>
namespace mission_adapters
{
/**
* @class NavigationClient
* @brief Đường duy nhất để mission layer điều khiển navigation.
*
* Gói này KHÔNG biết navigation runtime nào đang chạy phía sau: bridge phía runtime hiện thực
* interface này, test hiện thực bằng fake. Nhờ vậy chiều phụ thuộc luôn một chiều và mission
* layer test được mà không cần robot.
*
* Bất biến an toàn: mọi đường thoát của một mission — bị thay thế, bị huỷ, emergency — đều phải
* đi qua @ref cancelActive. Mission layer quên mission mà không bảo navigation dừng thì robot
* vẫn chạy tiếp tới goal cũ.
*/
class NavigationClient
{
public:
virtual ~NavigationClient() = default;
/**
* @brief Giao một mission cho navigation chạy.
* @param mission Mission bất biến, luôn khác null.
* @return false nếu navigation từ chối (goal không hợp lệ, chưa initialize...). Khi đó
* mission layer ghi nhận chặng này thất bại thay vì chờ vô hạn.
*/
virtual bool dispatch(const std::shared_ptr<const Mission>& mission) = 0;
/**
* @brief Yêu cầu dừng mission đang chạy.
* @param id Mission cần dừng — navigation bỏ qua nếu nó đã chạy sang mission khác.
*
* Kết quả cuối cùng vẫn quay về qua đường outcome bình thường (navDoneEvent /
* navFailedEvent mang cùng @p id), không phải qua giá trị trả về của hàm này.
*/
virtual void cancelActive(MissionId id) = 0;
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_NAVIGATION_CLIENT_H_

94
include/mission_adapters/plugin_registry.h Normal file
View File

@@ -0,0 +1,94 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Nạp và tra cứu MissionSourceAdapter theo schema.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_PLUGIN_REGISTRY_H_
#define MISSION_ADAPTERS_PLUGIN_REGISTRY_H_
#include <functional>
#include <map>
#include <string>
#include <vector>
#include <robot/node_handle.h>
#include <mission_adapters/adapter.h>
namespace mission_adapters
{
/**
* @class PluginRegistry
* @brief Bảng tra schema -> adapter, nạp từ YAML bằng Boost.DLL.
*
* Cấu hình mong đợi (xem `pnkx_nav_core/config/mission_adapters_params.yaml`):
*
* @code{.yaml}
* mission_adapters:
* mission_sources:
* - {name: goal_src, type: GoalSourceAdapter}
* - {name: vda5050_src, type: VDA5050SourceAdapter}
*
* GoalSourceAdapter:
* library_path: libmission_adapters_goal_source
* @endcode
*
* Khoá `library_path` là thứ hay bị quên nhất: thiếu nó thì plugin build xong vẫn báo "không
* tìm thấy" lúc chạy. Registry vì thế báo lỗi nêu đích danh khoá bị thiếu thay vì chỉ nói
* không nạp được.
*/
class PluginRegistry
{
public:
PluginRegistry() = default;
~PluginRegistry();
PluginRegistry(const PluginRegistry&) = delete;
PluginRegistry& operator=(const PluginRegistry&) = delete;
/**
* @brief Nạp toàn bộ nguồn khai trong `<ns>/mission_sources`.
* @param nh NodeHandle gốc.
* @param ns Namespace chứa danh sách nguồn.
* @return false nếu có bất kỳ nguồn nào không nạp được. Các nguồn còn lại vẫn được đăng ký,
* và mỗi lỗi được log kèm lý do cụ thể.
*/
bool loadFromConfig(robot::NodeHandle& nh, const std::string& ns = "mission_adapters");
/**
* @brief Đăng ký một adapter đã dựng sẵn (test, hoặc nguồn biên dịch thẳng vào host).
* @return false nếu adapter null, schema rỗng, hoặc schema đã có chủ.
*/
bool registerAdapter(const MissionSourceAdapter::Ptr& adapter);
/// @brief Tìm adapter xử lý schema này. nullptr nếu không có.
MissionSourceAdapter* find(const std::string& schema) const;
size_t size() const { return adapters_.size(); }
/// @brief Danh sách schema đã đăng ký, dùng cho log và test.
std::vector<std::string> schemas() const;
void clear();
private:
/// Nạp một nguồn. 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);
std::map<std::string, MissionSourceAdapter::Ptr> adapters_;
/**
* Giữ factory của Boost.DLL sống đúng bằng vòng đời registry.
*
* Đây 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 các adapter tạo từ nó vẫn còn sống — vtable trỏ vào vùng nhớ đã gỡ.
*/
std::vector<std::function<MissionSourceAdapter::Ptr()>> factories_;
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_PLUGIN_REGISTRY_H_

144
include/mission_adapters/types.h
View File

@@ -0,0 +1,144 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Kiểu dữ liệu nền của mission layer: trạng thái, mission, action.
*
* Author: DuongTD
*********************************************************************/
#ifndef MISSION_ADAPTERS_TYPES_H_
#define MISSION_ADAPTERS_TYPES_H_
#include <cstdint>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_protocol_msgs/Order.h>
namespace mission_adapters
{
/**
* @brief Danh tính của một mission, do MissionManager cấp khi submit và đơn điệu tăng.
*
* Mọi kết quả từ phía navigation phải mang ID này. Không có nó thì outcome trễ của một mission
* đã bị thay thế sẽ được ghi nhận cho mission đang chạy — mission mới "hoàn thành" mà robot
* chưa hề đi.
*/
using MissionId = std::uint64_t;
/// @brief Mission chưa được submit (hoặc không tồn tại).
static constexpr MissionId kInvalidMissionId = 0;
/// @brief Trạng thái hàng đợi mission. Tại mỗi thời điểm robot chỉ ở đúng một trạng thái.
enum class MissionState
{
IDLE,
QUEUED,
RUNNING,
PAUSED,
COMPLETED,
FAILED,
CANCELLED,
EMERGENCY,
CLEAR_EMERGENCY
};
/// @brief Tên trạng thái, dùng cho log và cho host hiển thị.
const char* toString(MissionState state);
/// @brief Nguồn gốc mission, quyết định cách consumer diễn giải nodes/edges.
enum class MissionType
{
SIMPLE_GOAL,
VDA5050_ORDER
};
/// @brief Action gắn với node (thực hiện tại chỗ) hay với edge (thực hiện dọc đường đi).
enum class ActionType
{
NODE_ACTION,
EDGE_ACTION
};
class Action
{
public:
int sequenceId = 0;
ActionType type = ActionType::NODE_ACTION;
robot_protocol_msgs::Action action;
};
/**
* @brief Một chặng việc self-contained mà navigation runtime có thể chạy trọn vẹn.
*
* @invariant Bất biến sau khi MissionManager::submit() gán @ref id: từ thời điểm đó mission chỉ
* được chia sẻ dưới dạng std::shared_ptr<const Mission>, nên ba thread (event, exec,
* navigation) đọc chung mà không cần khoá.
*/
class Mission
{
public:
MissionId id = kInvalidMissionId; ///< 0 cho tới khi submit; sau đó không đổi
MissionType type = MissionType::SIMPLE_GOAL;
/**
* false = mission chỉ-có-action: navigation runtime bỏ qua phần di chuyển và vào thẳng
* thực thi action (D8). @ref start và @ref goal khi đó không mang ý nghĩa.
*
* @invariant has_goal == false thì @ref actions phải khác rỗng — mission không goal cũng
* không action là mission không có việc gì để làm.
*/
bool has_goal = true;
robot_geometry_msgs::PoseStamped start; ///< chỉ hợp lệ khi has_goal
robot_geometry_msgs::PoseStamped goal; ///< chỉ hợp lệ khi has_goal
std::vector<robot_protocol_msgs::Node> nodes;
std::vector<robot_protocol_msgs::Edge> edges;
/**
* Action của chặng, đã sắp theo sequenceId.
*
* Mission layer KHÔNG diễn giải `actionType` và không lọc gì — đó là việc của navigation
* runtime (D8). Ở đây chúng chỉ đi qua nguyên vẹn, đúng thứ tự.
*/
std::vector<Action> actions;
/**
* Profile navigation hiệu lực của chặng có goal: `"position"`, `"docking"`,
* `"go_straight"` hoặc `"rotate"`. Mission chỉ-action phải để rỗng. Adapter hiện tại
* luôn ghi `"position"` rõ ràng cho chặng nav thường để trace/runtime biết chính xác
* planner nào sẽ được chọn; bridge vẫn chấp nhận rỗng như `position` để tương thích output
* cũ.
*
* Là **chuỗi** chứ không phải enum, và mission layer **không diễn giải** nó — cùng quy tắc
* với @ref actions. Mission layer không được biết tới navigation nên không có kiểu nào để
* diễn tả "profile"; lớp nối phía navigation mới dịch chuỗi này sang khái niệm của mình.
*/
std::string motion_hint;
/**
* Marker chọn cặp planner docking. Rỗng hoặc không có override cấu hình thì navigation
* dùng profile docking mặc định. Đây không phải goal_frame: marker là khoá cấu hình, còn
* goal_frame là frame TF được resolve khi chặng bắt đầu.
*/
std::string marker;
/**
* Đích lấy từ TF frame này thay vì từ @ref goal. Rỗng = dùng @ref goal.
*
* Chặng dò-rồi-tiến-vào không biết đích lúc được sinh ra: bước dò phía trước tạo ra frame
* này, và navigation tra nó tại thời điểm chặng được nhận.
*/
std::string goal_frame;
/// Quãng đường tương đối [m] so với pose hiện tại; dương = tiến, âm = lùi. NaN = không dùng.
double relative_distance = std::numeric_limits<double>::quiet_NaN();
};
} // namespace mission_adapters
#endif // MISSION_ADAPTERS_TYPES_H_