temporary storage

plugins/back_up_recovery.cpp

@@ -0,0 +1,153 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — per-cycle backup 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 BackUpRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  BackUpRecovery() = 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] BackUpRecovery '%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("backup_distance", backup_distance_, 0.5);
+    private_nh.param("linear_speed", linear_speed_, 0.1);
+    private_nh.param("require_costmap", require_costmap_, false);
+
+    if (!std::isfinite(backup_distance_) || backup_distance_ <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
+                         name_.c_str());
+      backup_distance_ = 0.5;
+    }
+    if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
+    {
+      robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
+                         name_.c_str());
+      linear_speed_ = 0.1;
+    }
+
+    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) || (require_costmap_ && local_costmap_ == nullptr))
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(),  );
+    }
+
+    elapsed_ += dt;
+    if (config_.timeout > 0.0 && elapsed_ > config_.timeout)
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    if (traveled_distance_ >= backup_distance_)
+    {
+      status_ = recovery_core::RecoveryStatus::kSucceeded;
+      return recovery_core::RecoveryResult::Velocity(zeroTwist(), status_);
+    }
+
+    robot_geometry_msgs::Twist command;
+    command.linear.x = -std::abs(linear_speed_);
+    traveled_distance_ = std::min(backup_distance_, traveled_distance_ + std::abs(command.linear.x) * dt);
+
+    if (traveled_distance_ >= backup_distance_)
+    {
+      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<BackUpRecovery>();
+  }
+
+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 backup_distance_ = 0.5;
+  double linear_speed_ = 0.1;
+  bool require_costmap_ = false;
+  double elapsed_ = 0.0;
+  double traveled_distance_ = 0.0;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::BackUpRecovery::create, BackUpRecovery)