optimal & fix file cmake

2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
--- a/plugins/clear_costmap_recovery.cpp
+++ b/plugins/clear_costmap_recovery.cpp
@@ -2,7 +2,7 @@
 *
 * Software License Agreement (BSD License)
 *
- * recovery_core — no-output clear costmap plugin.
+ * recovery_core — xoá vật cản đã tích trong costmap. Một tick, không output.
 *
 * Author: DuongTD
 *********************************************************************/
@@ -19,12 +19,16 @@
 #include <boost/pointer_cast.hpp>
 #include <boost/thread/locks.hpp>
 #include <robot/robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_costmap_2d/costmap_layer.h>

 namespace recovery_plugins
 {
 namespace
 {
+constexpr double kDefaultResetDistance = 3.0;  // [m]
+constexpr double kMaxResetDistance = 100.0;    // [m] trần vệ sinh cho param sai
+
 std::string leafName(std::string name)
 {
  const std::string::size_type slash = name.rfind('/');
@@ -34,13 +38,23 @@ std::string leafName(std::string name)
  }
  return name;
 }
-
-bool isValidResetDistance(double value)
-{
-  return std::isfinite(value) && value > 0.0;
-}
 }  // namespace

+/**
+ * @class ClearCostmapRecovery
+ * @brief Xoá vùng vật cản đã tích trong các layer được chỉ định.
+ *
+ * Dùng **hai instance** trong bộ default:
+ *  - `conservative_reset` (`invert_area_to_clear: false`) xoá vùng gần robot;
+ *  - `aggressive_reset`  (`invert_area_to_clear: true`) xoá mọi thứ **ngoài** vùng đó.
+ *
+ * Không phát output, hoàn tất trong một tick.
+ *
+ * @note Cố ý **không** gọi `Costmap2DROBOT::updateMap()`. Hàm đó giữ mutex master rồi chạy toàn bộ
+ *       chuỗi layer `updateBounds`/`updateCosts` — cỡ chục tới trăm ms — trong khi behavior này
+ *       chạy trên thread phát cmd_vel ở 30 Hz. Costmap tự update ở chu kỳ riêng của nó ngay sau
+ *       đó; ép update tại đây chỉ để đổi lấy một chu kỳ control bị lỡ.
+ */
 class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
 {
 public:
@@ -51,82 +65,107 @@ public:
    return std::make_shared<ClearCostmapRecovery>();
  }

-protected:
-  void onConfigure() override
+  recovery_core::RecoveryOutputType outputKind() const override
  {
-    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"));
+    return recovery_core::RecoveryOutputType::kNone;
+  }

-    if (!isValidResetDistance(reset_distance_))
+protected:
+  bool onConfigure(robot::NodeHandle& nh) override
+  {
+    nh.param("reset_distance", reset_distance_, kDefaultResetDistance);
+    nh.param("invert_area_to_clear", invert_area_to_clear_, false);
+    nh.param("affected_maps", affected_maps_, std::string("both"));
+
+    if (!std::isfinite(reset_distance_) || reset_distance_ <= 0.0 ||
+        reset_distance_ > kMaxResetDistance)
    {
-      robot::log_warning("[recovery_core] Invalid reset_distance for '%s'; using 3.0 m.",
-                         name_.c_str());
-      reset_distance_ = 3.0;
+      robot::log_warning("[recovery_core] '%s': reset_distance=%.3f m outside (0, %.0f]; using "
+                         "%.3f m.", name().c_str(), reset_distance_, kMaxResetDistance,
+                         kDefaultResetDistance);
+      reset_distance_ = kDefaultResetDistance;
    }

    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());
+      robot::log_warning("[recovery_core] '%s': affected_maps='%s' is invalid; using 'both'.",
+                         name().c_str(), affected_maps_.c_str());
      affected_maps_ = "both";
    }

-    std::vector<std::string> clearable_layers_default;
-    clearable_layers_default.emplace_back("obstacles");
+    std::vector<std::string> clearable_layers_default{"obstacles"};
    std::vector<std::string> clearable_layers;
-    private_nh.param("layer_names", clearable_layers, clearable_layers_default);
+    nh.param("layer_names", clearable_layers, clearable_layers_default);
    clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
+
+    if (clearable_layers_.empty())
+    {
+      robot::log_error("[recovery_core] '%s': layer_names is empty — this behavior will not clear "
+                       "anything.",
+                       name().c_str());
+      return false;
+    }
+
+    const bool needs_global = affected_maps_ == "global" || affected_maps_ == "both";
+    const bool needs_local = affected_maps_ == "local" || affected_maps_ == "both";
+
+    if (needs_global && ctx().global_costmap == nullptr)
+    {
+      robot::log_error("[recovery_core] '%s': affected_maps='%s' but the global costmap is "
+                       "missing.",
+                       name().c_str(), affected_maps_.c_str());
+      return false;
+    }
+    if (needs_local && ctx().local_costmap == nullptr)
+    {
+      robot::log_error("[recovery_core] '%s': affected_maps='%s' but the local costmap is missing.",
+                       name().c_str(), affected_maps_.c_str());
+      return false;
+    }
+
+    return true;
  }

-  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
+  bool onStart(const recovery_core::RecoveryGoal& /*goal*/) override
  {
-    // One-shot: công việc thực hiện ở onUpdate() lần đầu.
-    return recovery_core::RecoveryResult::Running().withMessage("clear costmap start");
+    // One-shot: công việc thực hiện ở tick đầu tiên, giữ start() không có tác dụng phụ.
+    return true;
  }

-  recovery_core::RecoveryResult onUpdate() override
+  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
  {
    bool ok = true;
+
    if (affected_maps_ == "global" || affected_maps_ == "both")
    {
-      ok = clear(ctx().global_costmap) && ok;
-      if (ok && force_updating_ && ctx().global_costmap != nullptr)
-      {
-        ctx().global_costmap->updateMap();
-      }
+      ok = clear(ctx().global_costmap, "global") && ok;
    }

    if (affected_maps_ == "local" || affected_maps_ == "both")
    {
-      ok = clear(ctx().local_costmap) && ok;
-      if (ok && force_updating_ && ctx().local_costmap != nullptr)
-      {
-        ctx().local_costmap->updateMap();
-      }
+      ok = clear(ctx().local_costmap, "local") && ok;
    }

-    return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete")
+    return ok ? recovery_core::RecoveryResult::Succeeded()
+                    .withProgress(1.0, 0.0)
+                    .withMessage("clear costmap complete")
              : recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
  }

 private:
-  bool clear(robot_costmap_2d::Costmap2DROBOT* costmap)
+  bool clear(robot_costmap_2d::Costmap2DROBOT* costmap, const char* which)
  {
    if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
    {
-      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap.",
-                       name_.c_str());
+      robot::log_error("[recovery_core] '%s': %s costmap is missing.", name().c_str(), which);
      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());
+      robot::log_error("[recovery_core] '%s': could not get the robot pose on the %s costmap.",
+                       name().c_str(), which);
      return false;
    }

@@ -134,12 +173,14 @@ private:
        costmap->getLayeredCostmap()->getPlugins();
    if (plugins == nullptr)
    {
-      robot::log_error("[recovery_core] ClearCostmapRecovery '%s' missing costmap layers.",
-                       name_.c_str());
+      robot::log_error("[recovery_core] '%s': %s costmap has no layer.", name().c_str(),
+                       which);
      return false;
    }

    bool touched_layer = false;
+    std::string available;
+
    for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
    {
      if (!plugin)
@@ -147,16 +188,23 @@ private:
        continue;
      }

-      const std::string name = leafName(plugin->getName());
-      if (clearable_layers_.count(name) == 0)
+      const std::string layer_name = leafName(plugin->getName());
+
+      if (!available.empty())
+      {
+        available += ", ";
+      }
+      available += layer_name;
+
+      if (clearable_layers_.count(layer_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());
+        robot::log_warning("[recovery_core] '%s': layer '%s' is not a CostmapLayer; skipped.",
+                           name().c_str(), layer_name.c_str());
        continue;
      }

@@ -165,13 +213,22 @@ private:
      touched_layer = true;
    }

-    return touched_layer;
+    if (!touched_layer)
+    {
+      // Nguyên nhân phổ biến nhất của "recovery này không làm gì" là sai tên layer trong config.
+      // Bản trước trả kFailed lặng lẽ, nên không có cách nào biết vì sao.
+      robot::log_error("[recovery_core] '%s': no layer of the %s costmap matches layer_names. "
+                       "Layers present: [%s].", name().c_str(), which, available.c_str());
+      return false;
+    }
+
+    return true;
  }

-  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& costmap,
-                double pose_x, double pose_y)
+  void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& layer, double pose_x,
+                double pose_y)
  {
-    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
+    boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(layer->getMutex()));

    const double start_point_x = pose_x - reset_distance_ / 2.0;
    const double start_point_y = pose_y - reset_distance_ / 2.0;
@@ -182,19 +239,20 @@ private:
    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);
+    layer->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+    layer->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());
+    layer->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
+
+    // Báo cho layer biết toàn bộ vùng của nó cần được ghi lại vào master ở chu kỳ update kế tiếp.
+    layer->addExtraBounds(layer->getOriginX(), layer->getOriginY(),
+                          layer->getOriginX() + layer->getSizeInMetersX(),
+                          layer->getOriginY() + layer->getSizeInMetersY());
  }

-  bool force_updating_ = false;
-  double reset_distance_ = 3.0;
-  bool invert_area_to_clear_ = false;
-  std::string affected_maps_ = "both";
+  double reset_distance_ = kDefaultResetDistance;  ///< [m] cạnh vùng vuông quanh robot
+  bool invert_area_to_clear_ = false;              ///< true = xoá phần NGOÀI vùng
+  std::string affected_maps_ = "both";             ///< local | global | both
  std::set<std::string> clearable_layers_;
 };