optimal & fix file cmake

test/mission_test_utils.h

@@ -0,0 +1,177 @@
+/*********************************************************************
+ *
+ * Helper dùng chung cho test của gói: dựng goal/order giả và chờ trạng thái.
+ *
+ *********************************************************************/
+#ifndef MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+#define MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_
+
+#include <chrono>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <mission_adapters/mission_adapters.h>
+
+namespace mission_test
+{
+
+/**
+ * @brief NavigationClient giả: ghi lại mọi lệnh xuống navigation để test kiểm thứ tự và số lần.
+ *
+ * Không mô phỏng chuyển động — test tự quyết định khi nào outcome quay về.
+ */
+class FakeNavigationClient : public mission_adapters::NavigationClient
+{
+public:
+    bool dispatch(const std::shared_ptr<const mission_adapters::Mission>& mission) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        dispatched_.push_back(mission);
+        return accept_dispatch_;
+    }
+
+    void cancelActive(mission_adapters::MissionId id) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        cancelled_.push_back(id);
+    }
+
+    /// Ép navigation từ chối mọi mission tiếp theo.
+    void setAcceptDispatch(bool accept)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        accept_dispatch_ = accept;
+    }
+
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_;
+    }
+
+    std::vector<mission_adapters::MissionId> cancelled() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return cancelled_;
+    }
+
+    size_t dispatchCount() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return dispatched_.size();
+    }
+
+private:
+    mutable std::mutex mutex_;
+    std::vector<std::shared_ptr<const mission_adapters::Mission>> dispatched_;
+    std::vector<mission_adapters::MissionId> cancelled_;
+    bool accept_dispatch_ = true;
+};
+
+
+/// @brief Goal hợp lệ: quaternion đã chuẩn hoá (w=1) để qua được validate() của adapter.
+inline robot_geometry_msgs::PoseStamped makeGoal(double x, double y)
+{
+    robot_geometry_msgs::PoseStamped goal;
+    goal.header.frame_id = "map";
+    goal.pose.position.x = x;
+    goal.pose.position.y = y;
+    goal.pose.orientation.w = 1.0;
+    return goal;
+}
+
+/// @brief Mission dựng thẳng, không qua adapter — dùng cho test của MissionManager/Executor.
+inline std::shared_ptr<mission_adapters::Mission> makeMission(double x, double y)
+{
+    auto mission = std::make_shared<mission_adapters::Mission>();
+    mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
+    mission->goal = makeGoal(x, y);
+    return mission;
+}
+
+/// @brief n mission độc lập, goal khác nhau.
+inline std::vector<std::shared_ptr<mission_adapters::Mission>> makeMissions(int count)
+{
+    std::vector<std::shared_ptr<mission_adapters::Mission>> missions;
+    missions.reserve(static_cast<size_t>(count));
+    for (int i = 0; i < count; ++i)
+        missions.push_back(makeMission(i, i));
+
+    return missions;
+}
+
+inline robot_protocol_msgs::Action makeAction(const std::string& id)
+{
+    robot_protocol_msgs::Action action;
+    action.actionId = id;
+    action.actionType = "TEST";
+    return action;
+}
+
+/// @brief Node đã released (base). Order thật của fleet manager luôn có ít nhất phần base.
+inline robot_protocol_msgs::Node makeNode(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Node node;
+    node.sequenceId = sequence_id;
+    node.nodeId = "node_" + std::to_string(sequence_id);
+    node.released = true;
+    node.nodePosition.x = sequence_id;
+    node.nodePosition.y = sequence_id;
+
+    if (add_action)
+        node.actions.push_back(makeAction("node_action_" + std::to_string(sequence_id)));
+
+    return node;
+}
+
+inline robot_protocol_msgs::Edge makeEdge(int sequence_id, bool add_action = false)
+{
+    robot_protocol_msgs::Edge edge;
+    edge.sequenceId = sequence_id;
+    edge.edgeId = "edge_" + std::to_string(sequence_id);
+    edge.released = true;
+
+    if (add_action)
+        edge.actions.push_back(makeAction("edge_action_" + std::to_string(sequence_id)));
+
+    return edge;
+}
+
+inline robot_protocol_msgs::Order makeOrder(int node_count, const std::string& order_id = "order_1",
+                                           std::uint32_t order_update_id = 0)
+{
+    robot_protocol_msgs::Order order;
+    order.orderId = order_id;
+    order.orderUpdateId = order_update_id;
+
+    for (int i = 0; i < node_count; ++i)
+        order.nodes.push_back(makeNode(i));
+
+    for (int i = 0; i < node_count - 1; ++i)
+        order.edges.push_back(makeEdge(i));
+
+    return order;
+}
+
+inline bool waitForState(mission_adapters::MissionManager& manager,
+                         mission_adapters::MissionState expected,
+                         std::chrono::milliseconds timeout)
+{
+    const auto deadline = std::chrono::steady_clock::now() + timeout;
+    while (std::chrono::steady_clock::now() < deadline)
+    {
+        if (manager.state() == expected)
+            return true;
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+
+    return manager.state() == expected;
+}
+
+}  // namespace mission_test
+
+#endif  // MISSION_ADAPTERS_TEST_MISSION_TEST_UTILS_H_