temporary storage 7/9/2026 19:41

plugins/regen_path_recovery.cpp

@@ -2,81 +2,59 @@
  *
  * Software License Agreement (BSD License)
  *
- * recovery_core — path output recovery plugin.
+ * recovery_core — path output recovery plugin (goal-driven, one-shot).
  *
  * Author: DuongTD
  *********************************************************************/
 
 #include <recovery_core/recovery_behavior.h>
 
-#include <string>
-#include <vector>
-
 #include <boost/dll/alias.hpp>
 #include <robot/robot.h>
 
 namespace recovery_plugins
 {
 
+/**
+ * @class RegenPathRecovery
+ * @brief Họ A (path output), one-shot: trả lại robot_nav_msgs::Path từ global_path hiện tại.
+ *
+ * Hoàn tất ngay ở lần update() đầu. Guard chưa configure/global_path null hoặc rỗng -> Failed().
+ */
 class RegenPathRecovery final : public recovery_core::RecoveryBehavior
 {
 public:
   RegenPathRecovery() = default;
 
-  void initialize(std::string name, tf3::BufferCore* tf,
-                  std::vector<robot_geometry_msgs::PoseStamped>* global_path,
-                  robot_costmap_2d::Costmap2DROBOT* global_costmap,
-                  robot_costmap_2d::Costmap2DROBOT* local_costmap) override
-  {
-    if (initialized_)
-    {
-      robot::log_error("[recovery_core] RegenPathRecovery '%s' initialized twice; ignoring.",
-                       name_.c_str());
-      return;
-    }
-
-    name_ = std::move(name);
-    tf_ = tf;
-    global_path_ = global_path;
-    global_costmap_ = global_costmap;
-    local_costmap_ = local_costmap;
-
-    initialized_ = true;
-    status_ = recovery_core::RecoveryStatus::kIdle;
-  }
-
-  recovery_core::RecoveryResult runBehavior() override
-  {
-    if (!initialized_ || global_path_ == nullptr || global_path_->empty())
-    {
-      status_ = recovery_core::RecoveryStatus::kFailed;
-      return recovery_core::RecoveryResult::Failed();
-    }
-
-    robot_nav_msgs::Path path;
-    path.poses = *global_path_;
-
-    status_ = recovery_core::RecoveryStatus::kSucceeded;
-    return recovery_core::RecoveryResult::PathOut(path, status_);
-  }
-
-  recovery_core::RecoveryStatus status() const override
-  {
-    return status_;
-  }
-
   static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
   {
     return std::make_shared<RegenPathRecovery>();
   }
 
-private:
-  tf3::BufferCore* tf_ = nullptr;
-  std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
-  robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
-  robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
-  bool initialized_ = false;
-  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+protected:
+  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
+  {
+    // One-shot: công việc thực hiện ở onUpdate() lần đầu, giữ start() gọn.
+    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
+                                                   recovery_core::RecoveryStatus::kRunning)
+        .withMessage("regen path start");
+  }
+
+  recovery_core::RecoveryResult onUpdate(double /*dt*/) override
+  {
+    const auto* global_path = ctx().global_path;
+    if (global_path == nullptr || global_path->empty())
+    {
+      return recovery_core::RecoveryResult::Failed().withMessage("no global path to regenerate");
+    }
+
+    robot_nav_msgs::Path path;
+    path.poses = *global_path;
+
+    return recovery_core::RecoveryResult::PathOut(path, recovery_core::RecoveryStatus::kSucceeded)
+        .withProgress(1.0, 0.0)
+        .withMessage("regen path complete");
+  }
 };
 
 }  // namespace recovery_plugins