temporary storage

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)