147 lines
6.7 KiB
C++
147 lines
6.7 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Nguồn mission từ VDA5050 Order (schema "vda5050.order").
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#ifndef MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
|
|
#define MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
|
|
|
|
#include <cstddef>
|
|
#include <limits>
|
|
#include <cstdint>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <robot/node_handle.h>
|
|
|
|
#include <mission_adapters/adapter.h>
|
|
|
|
namespace mission_plugins
|
|
{
|
|
|
|
/**
|
|
* @class VDA5050SourceAdapter
|
|
* @brief Cắt một VDA5050 Order thành các chặng, mỗi chặng kết thúc tại một node có action.
|
|
*
|
|
* Ba điểm conformance mà adapter này chịu trách nhiệm:
|
|
*
|
|
* 1. **base / horizon** — chỉ phần `released == true` được thực thi. Horizon là dự định của fleet
|
|
* manager, chưa được phép chạy; robot đi vào đó là đi vào đoạn đường chưa ai cho phép.
|
|
* 2. **orderId / orderUpdateId** — phân biệt yêu cầu MỚI (thay hàng đợi) với bản CẬP NHẬT của yêu
|
|
* cầu đang chạy (nối tiếp). Nhầm hai thứ này thì mỗi lần fleet manager release thêm horizon,
|
|
* robot lại huỷ và chạy lại chặng đang đi.
|
|
* 3. **goal / start self-contained** — mission mang sẵn pose đích, consumer không phải tự đoán từ
|
|
* `nodes.back()`.
|
|
*
|
|
* State là member, không phải static local: nhiều instance trong cùng tiến trình không được nhìn
|
|
* thấy order của nhau.
|
|
*/
|
|
class VDA5050SourceAdapter : public mission_adapters::MissionSourceAdapter
|
|
{
|
|
public:
|
|
/// @brief Factory được PluginRegistry nạp qua boost::dll::import_alias.
|
|
static mission_adapters::MissionSourceAdapter::Ptr create();
|
|
|
|
/**
|
|
* @brief Đọc param riêng của instance.
|
|
*
|
|
* Param (namespace `<name>`):
|
|
* - `global_frame` [string, mặc định "map"]: frame gán cho pose sinh ra từ nodePosition.
|
|
* - `compound_actions` [map, optional]: action "phải dò rồi mới biết đích" — xem @ref Step.
|
|
*/
|
|
bool configure(const std::string& name, robot::NodeHandle& nh) override;
|
|
|
|
std::string schema() const override;
|
|
|
|
bool validate(const mission_adapters::MissionRequest& request,
|
|
std::string& reason) const override;
|
|
|
|
mission_adapters::ConversionResult
|
|
convert(const mission_adapters::MissionRequest& request) override;
|
|
|
|
private:
|
|
/**
|
|
* @struct Step
|
|
* @brief Một bước trong chuỗi mở rộng của compound action.
|
|
*
|
|
* Engine hiểu đúng bốn từ khoá và **không biết** `charge`, `dock_target` hay `LiftFork` nghĩa là
|
|
* gì — đó là điều kiện để cùng một engine dùng lại cho model robot khác: thêm model = thêm YAML
|
|
* + một `.so` handler, không sửa dòng nào ở đây.
|
|
*
|
|
* Mỗi step phải có ĐÚNG MỘT trong bốn khoá đầu; sai cú pháp thì `configure()` từ chối lúc **boot**.
|
|
*/
|
|
struct Step
|
|
{
|
|
std::string action; ///< `action: <type>` -> chặng chỉ-action
|
|
std::string move_to; ///< `move_to: <frame>` -> chặng nav, frame cố định
|
|
std::string move_to_param; ///< `move_to_param: <key>`-> frame lấy từ actionParameters
|
|
double move = std::numeric_limits<double>::quiet_NaN(); ///< `move: <m>` tương đối
|
|
|
|
std::string motion_hint; ///< `profile:` -> position | docking | go_straight | rotate
|
|
std::string marker; ///< `marker:` -> khoá override planner, chỉ hợp lệ với docking
|
|
};
|
|
|
|
/// @brief Chuỗi chặng thay cho một action. Bảng chỉ giữ CẤU TRÚC, dữ liệu tới từ order.
|
|
using Steps = std::vector<Step>;
|
|
|
|
/// @brief Nạp bảng `compound_actions`. Trả false nếu bảng có mà khai sai — lỗi nổ lúc boot.
|
|
bool loadCompoundActions(const std::string& name, robot::NodeHandle& nh);
|
|
|
|
/// @brief Tra bảng theo actionType. nullptr nếu là action thường.
|
|
const Steps* findCompound(const std::string& action_type) const;
|
|
|
|
/**
|
|
* @brief Mở rộng action của một chặng thành chuỗi chặng, giữ nguyên thứ tự mảng JSON.
|
|
* @param leg Chặng nav tới node; action compound sẽ được GỠ khỏi nó.
|
|
* @param out Nơi nối thêm chặng sinh ra.
|
|
* @return false nếu thiếu tham số cấu trúc — bên gọi phải bỏ CẢ order.
|
|
*
|
|
* Action thường tích vào bộ đệm; gặp compound thì xả bộ đệm rồi phát chuỗi. Bộ đệm ĐẦU TIÊN nằm
|
|
* lại trên chặng nav, các bộ đệm sau thành chặng chỉ-action. Nhờ vậy `[MutedOn, charge, MutedOff]`
|
|
* cho ra `MutedOff` **sau** chuỗi charge — gom hết vào chặng nav sẽ bật lại cảm biến an toàn
|
|
* trước khi robot lùi vào trạm.
|
|
*/
|
|
bool expandCompound(const std::shared_ptr<mission_adapters::Mission>& leg,
|
|
std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const;
|
|
|
|
/// Số node đầu tiên có released == true. 0 nghĩa là không node nào được release.
|
|
static size_t countReleasedNodes(const robot_protocol_msgs::Order& order);
|
|
|
|
/// Số edge thuộc phần base, không vượt quá `released_node_count - 1`.
|
|
static size_t countReleasedEdges(const robot_protocol_msgs::Order& order,
|
|
size_t released_node_count);
|
|
|
|
/**
|
|
* @brief Phần order được phép thực thi.
|
|
* @return số node base; 0 nếu order không dùng trường `released` (xem ghi chú trong .cpp).
|
|
*/
|
|
size_t executableNodeCount(const robot_protocol_msgs::Order& order) const;
|
|
|
|
/// Dựng PoseStamped từ nodePosition (theta [rad] -> quaternion quanh trục z).
|
|
robot_geometry_msgs::PoseStamped toPose(const robot_protocol_msgs::Node& node) const;
|
|
|
|
std::string name_;
|
|
|
|
/// Frame gán cho goal/start. VDA5050 `mapId` là danh tính bản đồ, KHÔNG phải frame TF.
|
|
std::string global_frame_ = "map";
|
|
|
|
/// actionType -> chuỗi chặng. Rỗng = không action nào cần mở rộng, adapter chạy như trước.
|
|
std::map<std::string, Steps> compound_actions_;
|
|
|
|
/// Order đang chạy — dùng để nhận ra order update so với order mới.
|
|
std::string last_order_id_;
|
|
std::uint32_t last_order_update_id_ = 0;
|
|
|
|
/// Số node base đã chuyển đổi của order đang chạy; điểm bắt đầu cho phần release thêm.
|
|
size_t converted_node_count_ = 0;
|
|
};
|
|
|
|
} // namespace mission_plugins
|
|
|
|
#endif // MISSION_ADAPTERS_PLUGINS_VDA5050_SOURCE_ADAPTER_H_
|