optimal & fix file cmake

plugins/goal_source_adapter.cpp

@@ -0,0 +1,91 @@
+#include "goal_source_adapter.h"
+
+#include <cmath>
+
+#include <boost/dll/alias.hpp>
+
+namespace mission_plugins
+{
+
+mission_adapters::MissionSourceAdapter::Ptr GoalSourceAdapter::create()
+{
+    return std::make_shared<GoalSourceAdapter>();
+}
+
+bool GoalSourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
+{
+    (void)nh;   // nguồn này chưa có param riêng
+    name_ = name;
+    return true;
+}
+
+std::string GoalSourceAdapter::schema() const
+{
+    return mission_adapters::schema::kPoseStamped;
+}
+
+bool GoalSourceAdapter::validate(const mission_adapters::MissionRequest& request,
+                                 std::string& reason) const
+{
+    if (!request.pose)
+    {
+        reason = "request is missing the pose payload";
+        return false;
+    }
+
+    const auto& position = request.pose->pose.position;
+    const auto& orientation = request.pose->pose.orientation;
+
+    // NaN/Inf từ host phải bị chặn ngay tại biên: lọt xuống dưới thì mọi phép so khoảng cách tới
+    // goal đều trả false và robot chạy tới khi có người bấm dừng.
+    if (!std::isfinite(position.x) || !std::isfinite(position.y) || !std::isfinite(position.z))
+    {
+        reason = "goal contains NaN/Inf in position";
+        return false;
+    }
+
+    if (!std::isfinite(orientation.x) || !std::isfinite(orientation.y) ||
+        !std::isfinite(orientation.z) || !std::isfinite(orientation.w))
+    {
+        reason = "goal contains NaN/Inf in orientation";
+        return false;
+    }
+
+    // Quaternion toàn 0 là lỗi hay gặp khi host quên set orientation — nó không phải "hướng bất kỳ",
+    // nó là dữ liệu hỏng.
+    const double norm_squared = orientation.x * orientation.x +
+                                orientation.y * orientation.y +
+                                orientation.z * orientation.z +
+                                orientation.w * orientation.w;
+
+    if (std::sqrt(norm_squared) < kMinQuaternionNorm)
+    {
+        reason = "goal has a quaternion that cannot be normalized (norm ~ 0)";
+        return false;
+    }
+
+    return true;
+}
+
+mission_adapters::ConversionResult
+GoalSourceAdapter::convert(const mission_adapters::MissionRequest& request)
+{
+    mission_adapters::ConversionResult result;
+
+    if (!request.pose)
+        return result;
+
+    auto mission = std::make_shared<mission_adapters::Mission>();
+    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
+    mission->goal = *request.pose;
+    mission->motion_hint = "position";
+
+    // Goal đơn lẻ luôn thay việc đang chạy: người dùng bấm một đích mới nghĩa là bỏ đích cũ.
+    result.mode = mission_adapters::SubmitMode::kReplace;
+    result.missions.push_back(mission);
+    return result;
+}
+
+}  // namespace mission_plugins
+
+BOOST_DLL_ALIAS(mission_plugins::GoalSourceAdapter::create, GoalSourceAdapter)

plugins/goal_source_adapter.h

@@ -0,0 +1,55 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * Nguồn mission từ một goal đơn lẻ (schema "geometry.pose_stamped").
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_
+#define MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <robot/node_handle.h>
+
+#include <mission_adapters/adapter.h>
+
+namespace mission_plugins
+{
+
+/**
+ * @class GoalSourceAdapter
+ * @brief Goal đơn lẻ từ host -> đúng một mission.
+ *
+ * Không có state giữa các lần gọi, nhưng vẫn không dùng biến static: một tiến trình có thể chạy
+ * nhiều instance (nhiều robot mô phỏng) và state static sẽ nối chúng lại với nhau.
+ */
+class GoalSourceAdapter : public mission_adapters::MissionSourceAdapter
+{
+public:
+    /// @brief Factory được PluginRegistry nạp qua boost::dll::import_alias.
+    static mission_adapters::MissionSourceAdapter::Ptr create();
+
+    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:
+    /// Dưới ngưỡng này thì quaternion coi như không mang hướng nào. [không đơn vị]
+    static constexpr double kMinQuaternionNorm = 1e-6;
+
+    std::string name_;
+};
+
+}  // namespace mission_plugins
+
+#endif  // MISSION_ADAPTERS_PLUGINS_GOAL_SOURCE_ADAPTER_H_

plugins/vda5050_source_adapter.cpp

@@ -0,0 +1,577 @@
+#include "vda5050_source_adapter.h"
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+#include <utility>
+
+#include <yaml-cpp/yaml.h>
+
+#include <boost/dll/alias.hpp>
+
+#include <robot/robot.h>
+
+namespace mission_plugins
+{
+namespace
+{
+bool isNavigationProfile(const std::string& profile)
+{
+    return profile.empty() || profile == "position" || profile == "docking" ||
+           profile == "go_straight" || profile == "rotate";
+}
+
+    using mission_adapters::Action;
+    using mission_adapters::ActionType;
+    using mission_adapters::ConversionResult;
+    using mission_adapters::Mission;
+    using mission_adapters::MissionType;
+    using mission_adapters::SubmitMode;
+
+    /// Gom action của edge và của node cuối vào mission, sắp theo sequenceId của VDA5050.
+    void collectActions(const std::shared_ptr<Mission>& mission)
+    {
+        for (const auto& edge : mission->edges)
+        {
+            for (const auto& action : edge.actions)
+            {
+                Action ma;
+                ma.sequenceId = edge.sequenceId;
+                ma.type       = ActionType::EDGE_ACTION;
+                ma.action     = action;
+                mission->actions.push_back(std::move(ma));
+            }
+        }
+
+        if (!mission->nodes.empty())
+        {
+            const auto& node = mission->nodes.back();
+            for (const auto& action : node.actions)
+            {
+                Action ma;
+                ma.sequenceId = node.sequenceId;
+                ma.type       = ActionType::NODE_ACTION;
+                ma.action     = action;
+                mission->actions.push_back(std::move(ma));
+            }
+        }
+
+        // stable_sort chứ không sort: `sequenceId` ở đây là của NODE/EDGE sở hữu action, nên mọi
+        // action trên cùng một node có khoá BẰNG NHAU. Với khoá bằng nhau, std::sort không bảo đảm
+        // giữ thứ tự — mà thứ tự đó chính là thứ tự trong mảng JSON, thứ VDA5050 quy định là thứ tự
+        // thực hiện. Bản cũ chạy đúng chỉ nhờ libstdc++ dùng insertion sort cho dải nhỏ.
+        std::stable_sort(mission->actions.begin(), mission->actions.end(),
+            [](const Action& a, const Action& b) {
+                return a.sequenceId < b.sequenceId;
+            });
+    }
+
+}  // namespace
+
+mission_adapters::MissionSourceAdapter::Ptr VDA5050SourceAdapter::create()
+{
+    return std::make_shared<VDA5050SourceAdapter>();
+}
+
+bool VDA5050SourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
+{
+    name_ = name;
+
+    nh.getParam(name + "/global_frame", global_frame_, std::string("map"));
+    if (global_frame_.empty())
+    {
+        robot::log_error("VDA5050SourceAdapter[%s]: global_frame is empty", name.c_str());
+        return false;
+    }
+
+    if (!loadCompoundActions(name, nh))
+        return false;
+
+    last_order_id_.clear();
+    last_order_update_id_ = 0;
+    converted_node_count_ = 0;
+    return true;
+}
+
+bool VDA5050SourceAdapter::loadCompoundActions(const std::string& name, robot::NodeHandle& nh)
+{
+    compound_actions_.clear();
+
+    YAML::Node table;
+    if (!nh.getParam(name + "/compound_actions", table) || !table.IsMap())
+        return true;   // Không khai bảng là hợp lệ: adapter chạy y như trước.
+
+    for (auto entry = table.begin(); entry != table.end(); ++entry)
+    {
+        const std::string action_type = entry->first.as<std::string>();
+        const YAML::Node& steps_node = entry->second["steps"];
+
+        if (action_type.empty() || !steps_node || !steps_node.IsSequence() || steps_node.size() == 0)
+        {
+            robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' has no 'steps' list",
+                             name.c_str(), action_type.c_str());
+            return false;
+        }
+
+        Steps steps;
+        for (std::size_t i = 0; i < steps_node.size(); ++i)
+        {
+            const YAML::Node& n = steps_node[i];
+            Step step;
+
+            try
+            {
+                if (n["action"])         step.action        = n["action"].as<std::string>();
+                if (n["move_to"])        step.move_to       = n["move_to"].as<std::string>();
+                if (n["move_to_param"])  step.move_to_param = n["move_to_param"].as<std::string>();
+                if (n["move"])           step.move          = n["move"].as<double>();
+                if (n["profile"])        step.motion_hint   = n["profile"].as<std::string>();
+                if (n["marker"])         step.marker        = n["marker"].as<std::string>();
+            }
+            catch (const YAML::Exception& ex)
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is malformed: %s",
+                                 name.c_str(), action_type.c_str(), i, ex.what());
+                return false;
+            }
+
+            const int keys = (step.action.empty() ? 0 : 1) + (step.move_to.empty() ? 0 : 1) +
+                             (step.move_to_param.empty() ? 0 : 1) +
+                             (std::isfinite(step.move) ? 1 : 0);
+            if (keys != 1)
+            {
+                // Không đúng một từ khoá thì không có cách diễn giải nào là hiển nhiên đúng. Chặn ở
+                // boot thay vì đoán lúc order đầu tiên tới.
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu must have exactly one of "
+                                 "action / move_to / move_to_param / move (found %d)",
+                                 name.c_str(), action_type.c_str(), i, keys);
+                return false;
+            }
+
+            if (!isNavigationProfile(step.motion_hint))
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu has invalid profile '%s'",
+                                 name.c_str(), action_type.c_str(), i, step.motion_hint.c_str());
+                return false;
+            }
+            if (!step.action.empty() && !step.motion_hint.empty())
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu is action-only and must "
+                                 "not set profile",
+                                 name.c_str(), action_type.c_str(), i);
+                return false;
+            }
+            if (!step.marker.empty() && step.motion_hint != "docking")
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: '%s' step %zu sets marker but is not "
+                                 "a docking navigation step",
+                                 name.c_str(), action_type.c_str(), i);
+                return false;
+            }
+
+            steps.push_back(step);
+        }
+
+        compound_actions_[action_type] = std::move(steps);
+    }
+
+    // Chống đệ quy: một step sinh ra actionType mà chính nó cũng là compound thì expander sẽ mở rộng
+    // output của mình — vòng lặp vô hạn lúc convert.
+    for (const auto& entry : compound_actions_)
+    {
+        for (const Step& step : entry.second)
+        {
+            if (!step.action.empty() && compound_actions_.count(step.action) != 0)
+            {
+                robot::log_error("VDA5050SourceAdapter[%s]: compound '%s' emits '%s' which is itself "
+                                 "a compound action — that would recurse",
+                                 name.c_str(), entry.first.c_str(), step.action.c_str());
+                return false;
+            }
+        }
+    }
+
+    robot::log_info("VDA5050SourceAdapter[%s]: %zu compound action(s) loaded", name.c_str(),
+                    compound_actions_.size());
+    return true;
+}
+
+const VDA5050SourceAdapter::Steps*
+VDA5050SourceAdapter::findCompound(const std::string& action_type) const
+{
+    const auto it = compound_actions_.find(action_type);
+    return it == compound_actions_.end() ? nullptr : &it->second;
+}
+
+std::string VDA5050SourceAdapter::schema() const
+{
+    return mission_adapters::schema::kVda5050Order;
+}
+
+size_t VDA5050SourceAdapter::countReleasedNodes(const robot_protocol_msgs::Order& order)
+{
+    // VDA5050: base là tiền tố của danh sách node — dừng ở node đầu tiên chưa release.
+    size_t count = 0;
+    while (count < order.nodes.size() && order.nodes[count].released)
+        ++count;
+
+    return count;
+}
+
+size_t VDA5050SourceAdapter::countReleasedEdges(const robot_protocol_msgs::Order& order,
+                                                size_t released_node_count)
+{
+    if (released_node_count < 2)
+        return 0;
+
+    size_t count = 0;
+    const size_t limit = std::min(order.edges.size(), released_node_count - 1);
+    while (count < limit && order.edges[count].released)
+        ++count;
+
+    return count;
+}
+
+size_t VDA5050SourceAdapter::executableNodeCount(const robot_protocol_msgs::Order& order) const
+{
+    const size_t released = countReleasedNodes(order);
+    if (released > 0)
+        return released;
+
+    // Không node nào released. Theo VDA5050 order phải có ít nhất một base node, nên trường hợp này
+    // gần như luôn là host không điền `released`. Coi cả order là base — im lặng không chạy gì sẽ
+    // khiến fleet manager chờ vô hạn mà không có dấu hiệu nào.
+    robot::log_warning("VDA5050SourceAdapter[%s]: order '%s' has no released node — treating the "
+                       "whole order as base (did the host leave the 'released' field out?)",
+                       name_.c_str(), order.orderId.c_str());
+    return order.nodes.size();
+}
+
+robot_geometry_msgs::PoseStamped
+VDA5050SourceAdapter::toPose(const robot_protocol_msgs::Node& node) const
+{
+    robot_geometry_msgs::PoseStamped pose;
+    pose.header.frame_id = global_frame_;
+    pose.pose.position.x = node.nodePosition.x;   // [m]
+    pose.pose.position.y = node.nodePosition.y;   // [m]
+    pose.pose.position.z = 0.0;
+
+    // theta [rad] quanh trục z -> quaternion.
+    const double half_theta = 0.5 * node.nodePosition.theta;
+    pose.pose.orientation.x = 0.0;
+    pose.pose.orientation.y = 0.0;
+    pose.pose.orientation.z = std::sin(half_theta);
+    pose.pose.orientation.w = std::cos(half_theta);
+
+    return pose;
+}
+
+bool VDA5050SourceAdapter::validate(const mission_adapters::MissionRequest& request,
+                                    std::string& reason) const
+{
+    if (!request.order)
+    {
+        reason = "request is missing the order payload";
+        return false;
+    }
+
+    const auto& order = *request.order;
+
+    if (order.nodes.empty())
+    {
+        reason = "order has no node";
+        return false;
+    }
+
+    // VDA5050: n node liên thông cần đúng n-1 edge. Thiếu edge nghĩa là đồ thị đứt đoạn, cắt chặng
+    // theo chỉ số sẽ lấy nhầm edge của đoạn khác.
+    if (order.nodes.size() > 1 && order.edges.size() < order.nodes.size() - 1)
+    {
+        reason = "fewer edges than nodes - 1";
+        return false;
+    }
+
+    for (const auto& node : order.nodes)
+    {
+        if (!std::isfinite(node.nodePosition.x) || !std::isfinite(node.nodePosition.y) ||
+            !std::isfinite(node.nodePosition.theta))
+        {
+            reason = "nodePosition contains NaN/Inf at node '" + node.nodeId + "'";
+            return false;
+        }
+    }
+
+    // Bản cập nhật cũ hơn (hoặc phát lại) của order đang chạy: từ chối thay vì chạy lại tuyến đường.
+    if (!order.orderId.empty() && order.orderId == last_order_id_ &&
+        order.orderUpdateId <= last_order_update_id_)
+    {
+        reason = "orderUpdateId " + std::to_string(order.orderUpdateId) +
+                 " is not newer than the running one (" + std::to_string(last_order_update_id_) + ")";
+        return false;
+    }
+
+    return true;
+}
+
+
+bool VDA5050SourceAdapter::expandCompound(
+    const std::shared_ptr<mission_adapters::Mission>& leg,
+    std::vector<std::shared_ptr<mission_adapters::Mission>>& out) const
+{
+    if (compound_actions_.empty() || leg->actions.empty())
+        return true;   // Không có gì để mở rộng — đường đi thường.
+
+    std::vector<Action> buffer;      // action thường đang chờ xả
+    std::vector<Action> on_the_leg;  // bộ đệm ĐẦU TIÊN: ở lại trên chặng nav
+    bool first_flush = true;
+    bool expanded_any = false;
+
+    // Chặng chỉ-action sinh ra từ bộ đệm sau lần xả đầu.
+    auto flush = [&](void) {
+        if (buffer.empty())
+            return;
+        if (first_flush)
+        {
+            on_the_leg = buffer;
+        }
+        else
+        {
+            auto extra = std::make_shared<Mission>();
+            extra->type = MissionType::VDA5050_ORDER;
+            extra->has_goal = false;
+            extra->actions = buffer;
+            out.push_back(std::move(extra));
+        }
+        buffer.clear();
+    };
+
+    for (const Action& entry : leg->actions)
+    {
+        // Compound chỉ áp cho NODE_ACTION: một chuỗi dò-rồi-tiến-vào không có nghĩa khi gắn vào một
+        // cạnh mà robot đang đi trên đó.
+        const Steps* steps = (entry.type == ActionType::NODE_ACTION)
+                                 ? findCompound(entry.action.actionType)
+                                 : nullptr;
+
+        if (steps == nullptr)
+        {
+            if (entry.type == ActionType::EDGE_ACTION &&
+                findCompound(entry.action.actionType) != nullptr)
+            {
+                robot::log_warning("VDA5050SourceAdapter[%s]: '%s' is a compound action but sits on "
+                                   "an EDGE — running it as a plain action",
+                                   name_.c_str(), entry.action.actionType.c_str());
+            }
+            buffer.push_back(entry);
+            continue;
+        }
+
+        flush();
+        first_flush = false;
+        expanded_any = true;
+
+        for (const Step& step : *steps)
+        {
+            auto sub = std::make_shared<Mission>();
+            sub->type = MissionType::VDA5050_ORDER;
+
+            if (!step.action.empty())
+            {
+                // Action nội bộ: mang NGUYÊN actionParameters của action gốc — handler cần biết dò
+                // trạm nào, và nó là chỗ duy nhất hiểu ý nghĩa các tham số đó.
+                Action generated;
+                generated.type = ActionType::NODE_ACTION;
+                generated.sequenceId = entry.sequenceId;
+                generated.action = entry.action;
+                generated.action.actionType = step.action;
+                generated.action.actionId = entry.action.actionId + "-" + step.action;
+
+                sub->has_goal = false;
+                sub->actions.push_back(std::move(generated));
+            }
+            else
+            {
+                sub->has_goal = true;
+                // Mission nav luôn tự mô tả profile hiệu lực. Action-only giữ chuỗi rỗng để không
+                // bao giờ bị hiểu nhầm là một yêu cầu điều khiển.
+                sub->motion_hint = step.motion_hint.empty() ? "position" : step.motion_hint;
+                sub->marker = step.marker;
+                sub->start = leg->start;
+                sub->goal = leg->goal;   // chỗ dựa; đích thật đến muộn qua goal_frame/relative_distance
+
+                if (!step.move_to.empty())
+                {
+                    sub->goal_frame = step.move_to;
+                }
+                else if (!step.move_to_param.empty())
+                {
+                    // Thay thế xảy ra tại CONVERT: runtime không bao giờ thấy placeholder, và thiếu
+                    // tham số thì order bị từ chối trước khi robot nhúc nhích.
+                    const auto it = std::find_if(
+                        entry.action.actionParameters.begin(), entry.action.actionParameters.end(),
+                        [&step](const robot_protocol_msgs::ActionParameter& p) {
+                            return p.key == step.move_to_param;
+                        });
+
+                    if (it == entry.action.actionParameters.end() || it->value.empty())
+                    {
+                        robot::log_error("VDA5050SourceAdapter[%s]: compound action '%s' (actionId "
+                                         "'%s') requires parameter '%s' but it is missing",
+                                         name_.c_str(), entry.action.actionType.c_str(),
+                                         entry.action.actionId.c_str(), step.move_to_param.c_str());
+                        return false;
+                    }
+                    sub->goal_frame = it->value;
+                }
+                else
+                {
+                    sub->relative_distance = step.move;
+                }
+            }
+
+            out.push_back(std::move(sub));
+        }
+    }
+
+    flush();
+
+    if (expanded_any)
+        leg->actions = on_the_leg;
+
+    return true;
+}
+
+ConversionResult VDA5050SourceAdapter::convert(const mission_adapters::MissionRequest& request)
+{
+    ConversionResult result;
+
+    std::string reason;
+    if (!validate(request, reason))
+    {
+        robot::log_warning("VDA5050SourceAdapter[%s]: dropping the order — %s", name_.c_str(),
+                           reason.c_str());
+        return result;
+    }
+
+    const auto& order = *request.order;
+
+    const size_t node_count = executableNodeCount(order);
+    const size_t edge_count = (countReleasedNodes(order) > 0)
+                                  ? countReleasedEdges(order, node_count)
+                                  : order.edges.size();
+
+    // Order update của đúng order đang chạy: chỉ sinh phần vừa được release thêm.
+    const bool is_update = !order.orderId.empty() && order.orderId == last_order_id_;
+
+    size_t start_node_idx = 0;
+    if (is_update)
+    {
+        if (converted_node_count_ >= node_count)
+        {
+            robot::log_info("VDA5050SourceAdapter[%s]: order '%s' update %u released no further "
+                            "node — there is no new work",
+                            name_.c_str(), order.orderId.c_str(),
+                            static_cast<unsigned>(order.orderUpdateId));
+
+            last_order_update_id_ = order.orderUpdateId;
+            return result;
+        }
+
+        // Node cuối đã chuyển đổi trở thành node xuất phát của chặng tiếp theo.
+        start_node_idx = converted_node_count_ - 1;
+        result.mode = SubmitMode::kAppend;
+    }
+
+    // Chặng được cắt tại mỗi node có action: robot chạy tới node đó rồi mới thực hiện action.
+    std::vector<size_t> action_node_indices;
+    for (size_t i = start_node_idx + 1; i < node_count; ++i)
+    {
+        if (!order.nodes[i].actions.empty())
+            action_node_indices.push_back(i);
+    }
+
+    // Node xuất phát có action (và chưa từng được chuyển đổi) cũng là một chặng — chặng chỉ-action.
+    if (!is_update && !order.nodes[start_node_idx].actions.empty())
+        action_node_indices.insert(action_node_indices.begin(), start_node_idx);
+
+    auto makeMission = [&](size_t from, size_t to) {
+        auto mission = std::make_shared<Mission>();
+        mission->type = MissionType::VDA5050_ORDER;
+
+        mission->nodes.assign(order.nodes.begin() + from, order.nodes.begin() + to + 1);
+
+        if (to > from)
+        {
+            const size_t edge_to = std::min(to, edge_count);
+            if (from < edge_to)
+                mission->edges.assign(order.edges.begin() + from, order.edges.begin() + edge_to);
+        }
+
+        // D8: chặng một node là chặng chỉ-có-action (action nằm ngay tại node xuất phát) — không có
+        // quãng đường nào để đi. Quy tắc thuần cấu trúc, adapter không cần biết robot đang ở đâu.
+        mission->has_goal = (to > from);
+
+        if (mission->has_goal)
+        {
+            // Mission self-contained: consumer không phải suy goal ra từ nodes (A4).
+            mission->start = toPose(order.nodes[from]);
+            mission->goal  = toPose(order.nodes[to]);
+            mission->motion_hint = "position";
+        }
+
+        collectActions(mission);
+        return mission;
+    };
+
+    // Mission không goal mà cũng không action là mission rỗng — không sinh ra nó.
+    auto isMeaningful = [](const std::shared_ptr<Mission>& mission) {
+        return mission->has_goal || !mission->actions.empty();
+    };
+
+    size_t segment_start = start_node_idx;
+
+    for (const size_t action_node_idx : action_node_indices)
+    {
+        auto mission = makeMission(segment_start, action_node_idx);
+
+        // Mở rộng TRƯỚC isMeaningful: hàm này gỡ action compound khỏi chặng, và một chặng chỉ-action
+        // sau khi gỡ có thể trở thành rỗng.
+        std::vector<std::shared_ptr<Mission>> expanded;
+        if (!expandCompound(mission, expanded))
+        {
+            // Thiếu tham số cấu trúc: bỏ CẢ order. Một order chạy nửa vời — robot tới trạm sạc rồi
+            // không sạc — nguy hiểm hơn là không chạy.
+            result.missions.clear();
+            return result;
+        }
+
+        if (isMeaningful(mission))
+            result.missions.push_back(std::move(mission));
+
+        for (auto& sub : expanded)
+            result.missions.push_back(std::move(sub));
+
+        segment_start = action_node_idx;
+    }
+
+    // Đoạn còn lại sau node có action cuối cùng.
+    if (segment_start + 1 < node_count)
+    {
+        auto mission = makeMission(segment_start, node_count - 1);
+        if (isMeaningful(mission))
+            result.missions.push_back(std::move(mission));
+    }
+
+    if (result.missions.empty())
+        return result;
+
+    last_order_id_        = order.orderId;
+    last_order_update_id_ = order.orderUpdateId;
+    converted_node_count_ = node_count;
+
+    return result;
+}
+
+}  // namespace mission_plugins
+
+BOOST_DLL_ALIAS(mission_plugins::VDA5050SourceAdapter::create, VDA5050SourceAdapter)

plugins/vda5050_source_adapter.h

@@ -0,0 +1,146 @@
+/*********************************************************************
+ *
+ * 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_