125 lines
3.6 KiB
C++
125 lines
3.6 KiB
C++
/*********************************************************************
|
|
* 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
|