optimal & fix file cmake

plugins/wait_recovery.cpp

@@ -0,0 +1,103 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — đợi tại chỗ, không phát output.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <cmath>
+#include <string>
+
+#include <boost/dll/alias.hpp>
+#include <robot/robot.h>
+
+namespace recovery_plugins
+{
+namespace
+{
+constexpr double kDefaultWaitDuration = 3.0;  // [s]
+constexpr double kMaxWaitDuration = 300.0;    // [s] trần vệ sinh cho param cấu hình sai.
+}  // namespace
+
+/**
+ * @class WaitRecovery
+ * @brief Đứng yên một khoảng thời gian rồi báo thành công.
+ *
+ * Đây là recovery **an toàn nhất** trong bộ default và nên đứng đầu danh sách: nó không di chuyển,
+ * không cần pose, không cần collision check. Với AMR/AGV trong kho, phần lớn tình huống chặn đường
+ * là vật cản động (người, xe khác) — đợi vài giây rồi lập plan lại giải quyết được đa số, trong khi
+ * mọi behavior khác đều bắt robot cử động trong lúc chưa biết chuyện gì đang xảy ra.
+ *
+ * Thời lượng: `goal.params["wait_duration"]` cho lượt này, ngược lại param `wait_duration`.
+ */
+class WaitRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  WaitRecovery() = default;
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<WaitRecovery>();
+  }
+
+  recovery_core::RecoveryOutputType outputKind() const override
+  {
+    return recovery_core::RecoveryOutputType::kNone;
+  }
+
+protected:
+  bool onConfigure(robot::NodeHandle& nh) override
+  {
+    nh.param("wait_duration", default_wait_duration_, kDefaultWaitDuration);
+    default_wait_duration_ = sanitizeDuration(default_wait_duration_, kDefaultWaitDuration);
+    return true;
+  }
+
+  bool onStart(const recovery_core::RecoveryGoal& goal) override
+  {
+    wait_duration_ = sanitizeDuration(goal.param("wait_duration", default_wait_duration_),
+                                      default_wait_duration_);
+    return true;
+  }
+
+  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
+  {
+    // elapsed() do base đo bằng đồng hồ thật, nên loop chạy chậm không làm sai thời lượng đợi.
+    const double waited = elapsed();
+
+    if (waited >= wait_duration_)
+    {
+      return recovery_core::RecoveryResult::Succeeded()
+          .withProgress(1.0, 0.0)
+          .withMessage("wait complete");
+    }
+
+    return recovery_core::RecoveryResult::Running()
+        .withProgress(waited / wait_duration_, wait_duration_ - waited)
+        .withMessage("waiting");
+  }
+
+private:
+  double sanitizeDuration(double value, double fallback) const
+  {
+    if (!std::isfinite(value) || value <= 0.0 || value > kMaxWaitDuration)
+    {
+      robot::log_warning("[recovery_core] '%s': wait_duration=%.3f s outside (0, %.0f]; using %.3f "
+                         "s.",
+                         name().c_str(), value, kMaxWaitDuration, fallback);
+      return fallback;
+    }
+    return value;
+  }
+
+  double default_wait_duration_ = kDefaultWaitDuration;  ///< [s]
+  double wait_duration_ = kDefaultWaitDuration;          ///< [s] mục tiêu lượt này
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::WaitRecovery::create, WaitRecovery)