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)