temporary storage 7/9/2026 19:41

plugins/rotate_recovery.cpp

@@ -2,18 +2,16 @@
  *
  * Software License Agreement (BSD License)
  *
- * recovery_core — per-cycle rotate recovery plugin.
+ * recovery_core — per-cycle rotate recovery plugin (goal-driven).
  *
  * Author: DuongTD
  *********************************************************************/
 
 #include <recovery_core/recovery_behavior.h>
-#include <recovery_core/recovery_config.h>
 
 #include <algorithm>
 #include <cmath>
 #include <string>
-#include <vector>
 
 #include <boost/dll/alias.hpp>
 #include <robot/robot.h>
@@ -22,90 +20,76 @@ namespace recovery_plugins
 {
 namespace
 {
-robot_geometry_msgs::Twist zeroTwist()
-{
-  return robot_geometry_msgs::Twist();
-}
-
-bool validCycle(double dt)
-{
-  return std::isfinite(dt) && dt > 0.0;
-}
+constexpr double kDefaultTargetAngle = 1.57079632679;  // pi/2 rad.
+constexpr double kDefaultAngularSpeed = 0.4;           // rad/s.
 }  // namespace
 
+/**
+ * @class RotateRecovery
+ * @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
+ *
+ * goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ
+ * góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả
+ * Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command).
+ */
 class RotateRecovery final : public recovery_core::RecoveryBehavior
 {
 public:
   RotateRecovery() = 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
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
   {
-    if (initialized_)
-    {
-      robot::log_error("[recovery_core] RotateRecovery '%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;
+    return std::make_shared<RotateRecovery>();
+  }
 
+protected:
+  void onConfigure() override
+  {
     robot::NodeHandle private_nh("~/" + name_);
-    config_ = recovery_core::RecoveryConfig::fromNodeHandle(private_nh);
-    private_nh.param("target_angle", target_angle_, 1.57079632679);
-    private_nh.param("angular_speed", angular_speed_, 0.4);
+    private_nh.param("target_angle", default_target_angle_, kDefaultTargetAngle);
+    private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
 
-    if (!std::isfinite(target_angle_) || std::abs(target_angle_) <= 0.0)
+    if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
     {
       robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
                          name_.c_str());
-      target_angle_ = 1.57079632679;
+      default_target_angle_ = kDefaultTargetAngle;
     }
-    if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
+    if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 0.0)
     {
       robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
                          name_.c_str());
-      angular_speed_ = 0.4;
+      default_angular_speed_ = kDefaultAngularSpeed;
     }
-
-    initialized_ = true;
-    status_ = recovery_core::RecoveryStatus::kIdle;
   }
 
-  recovery_core::RecoveryResult runBehavior() override
+  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
   {
-    status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
-                             recovery_core::RecoveryStatus::kFailed;
-    return initialized_ ? recovery_core::RecoveryResult::Running() :
-                          recovery_core::RecoveryResult::Failed();
+    // Góc đích: goal.angle nếu hợp lệ, ngược lại default configured.
+    target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0)
+                        ? goal.angle
+                        : default_target_angle_;
+
+    angular_speed_ = std::abs(goal.param("angular_speed", default_angular_speed_));
+    if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
+    {
+      angular_speed_ = default_angular_speed_;
+    }
+
+    rotated_angle_ = 0.0;
+    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
+                                                   recovery_core::RecoveryStatus::kRunning)
+        .withProgress(0.0, std::abs(target_angle_))
+        .withMessage("rotate start");
   }
 
-  recovery_core::RecoveryResult computeCommand(double dt) override
+  recovery_core::RecoveryResult onUpdate(double dt) override
   {
-    if (!initialized_ || !validCycle(dt))
-    {
-      status_ = recovery_core::RecoveryStatus::kFailed;
-      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
-    }
-
-    elapsed_ += dt;
-    if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
-    {
-      status_ = recovery_core::RecoveryStatus::kFailed;
-      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
-    }
-
     const double target = std::abs(target_angle_);
+
     if (rotated_angle_ >= target)
     {
-      status_ = recovery_core::RecoveryStatus::kSucceeded;
-      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+      return succeeded(target);
     }
 
     robot_geometry_msgs::Twist command;
@@ -114,36 +98,29 @@ public:
 
     if (rotated_angle_ >= target)
     {
-      status_ = recovery_core::RecoveryStatus::kSucceeded;
-      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+      return succeeded(target);
     }
 
-    status_ = recovery_core::RecoveryStatus::kRunning;
-    return recovery_core::RecoveryResult::Velocity(command, status_);
-  }
-
-  recovery_core::RecoveryStatus status() const override
-  {
-    return status_;
-  }
-
-  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
-  {
-    return std::make_shared<RotateRecovery>();
+    return recovery_core::RecoveryResult::Velocity(command,
+                                                   recovery_core::RecoveryStatus::kRunning)
+        .withProgress(rotated_angle_ / target, target - rotated_angle_)
+        .withMessage("rotating");
   }
 
 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;
-  recovery_core::RecoveryConfig config_;
-  bool initialized_ = false;
-  recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
+  recovery_core::RecoveryResult succeeded(double target)
+  {
+    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
+                                                   recovery_core::RecoveryStatus::kSucceeded)
+        .withProgress(1.0, 0.0)
+        .withMessage("rotate complete");
+  }
 
-  double target_angle_ = 1.57079632679;
-  double angular_speed_ = 0.4;
-  double elapsed_ = 0.0;
+  double default_target_angle_ = kDefaultTargetAngle;
+  double default_angular_speed_ = kDefaultAngularSpeed;
+
+  double target_angle_ = kDefaultTargetAngle;
+  double angular_speed_ = kDefaultAngularSpeed;
   double rotated_angle_ = 0.0;
 };