temporary storage

plugins/regen_path_recovery.cpp

@@ -0,0 +1,84 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — path output recovery plugin.
+ *
+ * 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 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;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)