temporary storage 7/9/2026 19:41

src/recovery_behavior.cpp

@@ -1,19 +1,118 @@
 /*********************************************************************
- * recovery_core — default impl cho RecoveryBehavior.
+ * recovery_core — phần chung (template-method) của RecoveryBehavior.
  *
- * Chỉ chứa default computeCommand() (họ A/B không override). initialize()/runBehavior()/
- * status() là thuần ảo, do plugin cụ thể triển khai.
+ * configure()/start()/update()/cancel() là NON-VIRTUAL: base lo guard vòng đời, timeout, đo
+ * elapsed, và xử lý cancel; plugin chỉ triển khai onConfigure()/onStart()/onUpdate().
  *
  * Author: DuongTD
  *********************************************************************/
 #include <recovery_core/recovery_behavior.h>
 
+#include <cmath>
+
+#include <robot/robot.h>
+
 namespace recovery_core
 {
-
-RecoveryResult RecoveryBehavior::computeCommand(double /*dt*/)
+namespace
 {
-  return RecoveryResult::Failed();
+robot_geometry_msgs::Twist zeroTwist()
+{
+  return robot_geometry_msgs::Twist();
+}
+
+bool validCycle(double dt)
+{
+  return std::isfinite(dt) && dt > 0.0;
+}
+}  // namespace
+
+void RecoveryBehavior::configure(const std::string& name, const RecoveryContext& ctx)
+{
+  if (configured_)
+  {
+    return;
+  }
+
+  name_ = name;
+  ctx_ = ctx;
+  status_ = RecoveryStatus::kIdle;
+
+  // Param chung duy nhất còn lại: timeout (s, 0 = không giới hạn).
+  robot::NodeHandle private_nh("~/" + name_);
+  private_nh.param("timeout", timeout_, 0.0);
+  if (!std::isfinite(timeout_) || timeout_ < 0.0)
+  {
+    robot::log_warning("[recovery_core] Invalid timeout for '%s'; using 0 (no timeout).",
+                       name_.c_str());
+    timeout_ = 0.0;
+  }
+
+  onConfigure();
+
+  configured_ = true;
+}
+
+RecoveryResult RecoveryBehavior::start(const RecoveryGoal& goal)
+{
+  if (!configured_)
+  {
+    status_ = RecoveryStatus::kFailed;
+    return RecoveryResult::Failed().withMessage("start() before configure()");
+  }
+
+  goal_ = goal;
+  elapsed_ = 0.0;
+  cancel_requested_ = false;
+  started_ = true;
+  status_ = RecoveryStatus::kRunning;
+
+  return onStart(goal_);
+}
+
+RecoveryResult RecoveryBehavior::update(double dt)
+{
+  if (!configured_ || !started_)
+  {
+    status_ = RecoveryStatus::kFailed;
+    return RecoveryResult::Failed().withMessage("update() before start()");
+  }
+
+  // Đã kết thúc: giữ nguyên trạng thái, không tick thêm.
+  if (status_ != RecoveryStatus::kRunning)
+  {
+    return RecoveryResult::Velocity(zeroTwist(), status_);
+  }
+
+  if (cancel_requested_)
+  {
+    status_ = RecoveryStatus::kCancelled;
+    return RecoveryResult::Velocity(zeroTwist(), status_)
+        .withMessage("cancelled by caller");
+  }
+
+  if (!validCycle(dt))
+  {
+    status_ = RecoveryStatus::kFailed;
+    return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("invalid dt");
+  }
+
+  elapsed_ += dt;
+  if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
+  {
+    status_ = RecoveryStatus::kFailed;
+    return RecoveryResult::Velocity(zeroTwist(), status_).withMessage("timeout");
+  }
+
+  RecoveryResult result = onUpdate(dt);
+  result.elapsed = elapsed_;
+  status_ = result.status;
+  return result;
+}
+
+void RecoveryBehavior::cancel()
+{
+  cancel_requested_ = true;
 }
 
 }  // namespace recovery_core