optimal & fix file cmake

adapters/costmap_collision_checker.cpp

@@ -0,0 +1,124 @@
+/*********************************************************************
+ * recovery_core — CollisionChecker dựng trên Costmap2DROBOT.
+ *
+ * Author: DuongTD
+ *********************************************************************/
+#include <recovery_core/adapters/costmap_collision_checker.h>
+
+#include <algorithm>
+#include <cmath>
+#include <utility>
+
+#include <robot_costmap_2d/cost_values.h>
+#include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/footprint.h>
+
+namespace recovery_core
+{
+
+void CostmapCollisionChecker::setFootprintOverride(
+    std::vector<robot_geometry_msgs::Point> footprint)
+{
+  footprint_override_ = std::move(footprint);
+}
+
+void CostmapCollisionChecker::setSampleStep(double step_m)
+{
+  if (std::isfinite(step_m) && step_m > 0.0)
+  {
+    sample_step_ = step_m;
+  }
+}
+
+double CostmapCollisionChecker::pointCost(const robot_costmap_2d::Costmap2DROBOT& costmap,
+                                          double wx, double wy) const
+{
+  // Lấy lại con trỏ lưới MỖI LẦN: LayeredCostmap có thể thay Costmap2D bên dưới giữa hai cycle.
+  const robot_costmap_2d::Costmap2D* grid = costmap.getCostmap();
+  if (grid == nullptr)
+  {
+    return kOutsideMap;
+  }
+
+  unsigned int mx = 0;
+  unsigned int my = 0;
+  if (!grid->worldToMap(wx, wy, mx, my))
+  {
+    return kOutsideMap;
+  }
+
+  const unsigned char cost = grid->getCost(mx, my);
+
+  if (cost == robot_costmap_2d::NO_INFORMATION)
+  {
+    return kUnknown;
+  }
+
+  // INSCRIBED_INFLATED_OBSTACLE nghĩa là tâm robot đặt ở đây thì đã chạm vật cản — chặn như lethal.
+  if (cost == robot_costmap_2d::LETHAL_OBSTACLE ||
+      cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+  {
+    return kLethal;
+  }
+
+  return static_cast<double>(cost);
+}
+
+double CostmapCollisionChecker::lineCost(const robot_costmap_2d::Costmap2DROBOT& costmap, double x0,
+                                         double y0, double x1, double y1) const
+{
+  const double length = std::hypot(x1 - x0, y1 - y0);
+  const int steps = std::max(1, static_cast<int>(std::ceil(length / sample_step_)));
+
+  double worst = 0.0;
+  for (int i = 0; i <= steps; ++i)
+  {
+    const double t = static_cast<double>(i) / static_cast<double>(steps);
+    const double cost = pointCost(costmap, x0 + t * (x1 - x0), y0 + t * (y1 - y0));
+    if (cost < 0.0)
+    {
+      return cost;  // Bất kỳ mã lỗi nào cũng chặn ngay, không đi tiếp trên đoạn này.
+    }
+    worst = std::max(worst, cost);
+  }
+  return worst;
+}
+
+double CostmapCollisionChecker::footprintCost(double x, double y, double theta) const
+{
+  if (costmap_ == nullptr)
+  {
+    // Không có costmap thì không khẳng định được chỗ này đi được — trả về "chặn" chứ không phải 0.
+    return kOutsideMap;
+  }
+
+  const std::vector<robot_geometry_msgs::Point>& spec =
+      footprint_override_.empty() ? costmap_->getRobotFootprint() : footprint_override_;
+
+  if (spec.size() < 3)
+  {
+    // Footprint suy biến -> coi robot là một điểm, giống CostmapModel.
+    return pointCost(*costmap_, x, y);
+  }
+
+  std::vector<robot_geometry_msgs::Point> oriented;
+  robot_costmap_2d::transformFootprint(x, y, theta, spec, oriented);
+
+  double worst = 0.0;
+  for (std::size_t i = 0; i < oriented.size(); ++i)
+  {
+    const std::size_t j = (i + 1) % oriented.size();
+    const double cost =
+        lineCost(*costmap_, oriented[i].x, oriented[i].y, oriented[j].x, oriented[j].y);
+    if (cost < 0.0)
+    {
+      return cost;
+    }
+    worst = std::max(worst, cost);
+  }
+
+  return worst;
+}
+
+}  // namespace recovery_core