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)

plugins/clear_costmap_recovery.cpp

@@ -0,0 +1,235 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ * recovery_core — no-output clear costmap plugin.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+
+#include <recovery_core/recovery_behavior.h>
+
+#include <algorithm>
+#include <cmath>
+#include <set>
+#include <string>
+#include <vector>
+
+#include <boost/dll/alias.hpp>
+#include <boost/pointer_cast.hpp>
+#include <boost/thread/locks.hpp>
+#include <robot/robot.h>
+#include <robot_costmap_2d/costmap_layer.h>
+
+namespace recovery_plugins
+{
+namespace
+{
+std::string leafName(std::string name)
+{
+  const std::string::size_type slash = name.rfind('/');
+  if (slash != std::string::npos)
+  {
+    name = name.substr(slash + 1);
+  }
+  return name;
+}
+
+bool isValidResetDistance(double value)
+{
+  return std::isfinite(value) && value > 0.0;
+}
+}  // namespace
+
+class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
+{
+public:
+  ClearCostmapRecovery() = 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] ClearCostmapRecovery '%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_);
+    private_nh.param("reset_distance", reset_distance_, 3.0);
+    private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
+    private_nh.param("force_updating", force_updating_, false);
+    private_nh.param("affected_maps", affected_maps_, std::string("both"));
+
+    if (!isValidResetDistance(reset_distance_))
+    {
+      robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.",
+                         name_.c_str());
+      reset_distance_ = 3.0;
+    }
+
+    if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
+    {
+      robot::log_warning("[recovery_core] Invalid affected_maps '%s' for '%s'; using 'both'.",
+                         affected_maps_.c_str(), name_.c_str());
+      affected_maps_ = "both";
+    }
+
+    std::vector<std::string> clearable_layers_default;
+    clearable_layers_default.emplace_back("obstacles");
+    std::vector<std::string> clearable_layers;
+    private_nh.param("layer_names", clearable_layers, clearable_layers_default);
+    clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
+
+    initialized_ = true;
+    status_ = recovery_core::RecoveryStatus::kIdle;
+  }
+
+  recovery_core::RecoveryResult runBehavior() override
+  {
+    if (!initialized_)
+    {
+      status_ = recovery_core::RecoveryStatus::kFailed;
+      return recovery_core::RecoveryResult::Failed();
+    }
+
+    bool ok = true;
+    if (affected_maps_ == "global" || affected_maps_ == "both")
+    {
+      ok = clear(global_costmap_) && ok;
+      if (ok && force_updating_ && global_costmap_ != nullptr)
+      {
+        global_costmap_->updateMap();
+      }
+    }
+
+    if (affected_maps_ == "local" || affected_maps_ == "both")
+    {
+      ok = clear(local_costmap_) && ok;
+      if (ok && force_updating_ && local_costmap_ != nullptr)
+      {
+        local_costmap_->updateMap();
+      }
+    }
+
+    status_ = ok ? recovery_core::RecoveryStatus::kSucceeded :
+                   recovery_core::RecoveryStatus::kFailed;
+    return ok ? recovery_core::RecoveryResult::Succeeded() :
+                recovery_core::RecoveryResult::Failed();
+  }
+
+  recovery_core::RecoveryStatus status() const override
+  {
+    return status_;
+  }
+
+  static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
+  {
+    return std::make_shared<ClearCostmapRecovery>();
+  }
+
+private:
+  bool clear(robot_costmap_2d::Costmap2DROBOT* costmap)
+  {
+    if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap.",
+                       name_.c_str());
+      return false;
+    }
+
+    robot_geometry_msgs::PoseStamped pose;
+    if (!costmap->getRobotPose(pose))
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' cannot get robot pose.",
+                       name_.c_str());
+      return false;
+    }
+
+    std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>* plugins =
+        costmap->getLayeredCostmap()->getPlugins();
+    if (plugins == nullptr)
+    {
+      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap layers.",
+                       name_.c_str());
+      return false;
+    }
+
+    bool touched_layer = false;
+    for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
+    {
+      if (!plugin)
+      {
+        continue;
+      }
+
+      const std::string name = leafName(plugin->getName());
+      if (clearable_layers_.count(name) == 0)
+      {
+        continue;
+      }
+
+      if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
+      {
+        robot::log_warning("[recovery_core] Layer '%s' is not a CostmapLayer; skipped.",
+                           name.c_str());
+        continue;
+      }
+
+      clearMap(boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin),
+               pose.pose.position.x, pose.pose.position.y);
+      touched_layer = true;
+    }
+
+    return touched_layer;
+  }
+
+  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap,
+                double pose_x, double pose_y)
+  {
+    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+
+    const double start_point_x = pose_x - reset_distance_ / 2.0;
+    const double start_point_y = pose_y - reset_distance_ / 2.0;
+    const double end_point_x = start_point_x + reset_distance_;
+    const double end_point_y = start_point_y + reset_distance_;
+
+    int start_x = 0;
+    int start_y = 0;
+    int end_x = 0;
+    int end_y = 0;
+    costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+    costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
+
+    costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
+    costmap->addExtraBounds(costmap->getOriginX(), costmap->getOriginY(),
+                            costmap->getOriginX() + costmap->getSizeInMetersX(),
+                            costmap->getOriginY() + costmap->getSizeInMetersY());
+  }
+
+  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;
+
+  bool force_updating_ = false;
+  double reset_distance_ = 3.0;
+  bool invert_area_to_clear_ = false;
+  std::string affected_maps_ = "both";
+  std::set<std::string> clearable_layers_;
+};
+
+}  // namespace recovery_plugins
+
+BOOST_DLL_ALIAS(recovery_plugins::ClearCostmapRecovery::create, ClearCostmapRecovery)

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)

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)