temporary storage

plugins/rotate_recovery.cpp

@@ -0,0 +1,152 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — per-cycle rotate recovery plugin.
+ *
+ * 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>
+
+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;
+}
+}  // namespace
+
+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
+  {
+    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;
+
+    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);
+
+    if (!std::isfinite(target_angle_) || std::abs(target_angle_) <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
+                         name_.c_str());
+      target_angle_ = 1.57079632679;
+    }
+    if (!std::isfinite(angular_speed_) || 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;
+    }
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    status_ = initialized_ ? recovery_core::RecoveryStatus::kRunning :
+                             recovery_core::RecoveryStatus::kFailed;
+    return initialized_ ? recovery_core::RecoveryResult::Running() :
+                          recovery_core::RecoveryResult::Failed();
+  }
+
+  recovery_core::RecoveryResult computeCommand(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_);
+    }
+
+    robot_geometry_msgs::Twist command;
+    command.angular.z = std::copysign(angular_speed_, target_angle_);
+    rotated_angle_ = std::min(target, rotated_angle_ + std::abs(command.angular.z) * dt);
+
+    if (rotated_angle_ >= target)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    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>();
+  }
+
+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;
+
+  double target_angle_ = 1.57079632679;
+  double angular_speed_ = 0.4;
+  double elapsed_ = 0.0;
+  double rotated_angle_ = 0.0;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, RotateRecovery)