DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

otimal

Browse Source
This commit is contained in:
duongtd
2026-07-23 14:47:27 +07:00
parent 03b13f6936
commit 0e84ac53cb
2 changed files with 273 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
include/robot_costmap_2d/voxel_layer.h
View File

@@ -51,6 +51,9 @@
#include <robot_costmap_2d/obstacle_layer.h>
#include <robot_voxel_grid/voxel_grid.h>
#include <limits>
#include <vector>
namespace robot_costmap_2d
{
@@ -102,11 +105,42 @@ private:
bool clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
double raytrace_range, unsigned int cell_raytrace_range,
double* min_x, double* min_y, double* max_x, double* max_y);
bool clearDepthColumns(double ox, double oy, double cover_distance, double far_distance,
double min_range, double max_range, double skip_dist,
double* min_x, double* min_y, double* max_x, double* max_y);
bool clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const;
bool publish_voxel_;
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_;
/// Clearing rays stop this far [m] before the measured surface.
/// Negative keeps the legacy 2 * resolution behaviour.
double frustum_skip_distance_ = -1.0;
/// Full-column clearing: per depth-image pixel column, the nearest return
/// inside the obstacle height band certifies "no obstacle in this direction
/// closer than d". Cells along that 2D beam get their whole voxel column
/// cleared, removing marked voxels the per-pixel 3D rays cannot reach
/// (above the vertical FOV at close range).
bool frustum_column_clearing_ = false;
/// Height band [m] used to detect the nearest in-band return. Points below
/// the band (floor) do not shorten the beam. max < 0: use max_obstacle_height.
double column_clear_min_height_ = 0.10;
double column_clear_max_height_ = -1.0;
/// Full columns are only cleared beyond this distance [m], where the
/// vertical FOV covers the whole height band. Negative: derive each frame
/// from the camera intrinsics and mounting pose.
double column_cover_distance_ = -1.0;
struct DepthColumnStat
{
double min_band_dist = -1.0; ///< horizontal distance of nearest in-band return; < 0 = none
double azimuth = 0.0; ///< beam direction in the global frame
double best_row_delta = std::numeric_limits<double>::infinity();
bool has_ray = false; ///< column had at least one readable pixel
};
std::vector<DepthColumnStat> depth_column_stats_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
std::vector<unsigned char> rolling_costmap_scratch_;

245
plugins/voxel_layer.cpp
View File

@@ -98,6 +98,11 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
mark_threshold_ = loadParam(layer, "mark_threshold", 0);
combination_method_ = loadParam(layer, "combination_method", 0.0);
frustum_skip_distance_ = loadParam(layer, "frustum_skip_distance", -1.0);
frustum_column_clearing_ = loadParam(layer, "frustum_column_clearing", false);
column_clear_min_height_ = loadParam(layer, "column_clear_min_height", 0.10);
column_clear_max_height_ = loadParam(layer, "column_clear_max_height", -1.0);
column_cover_distance_ = loadParam(layer, "column_cover_distance", -1.0);
int size_z, unknown_threshold, mark_threshold, frustum_pixel_step;
if (nh.hasParam("enabled"))
@@ -125,6 +130,26 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
}
if (nh.hasParam("combination_method"))
nh.getParam("combination_method", combination_method_);
if (nh.hasParam("frustum_skip_distance"))
nh.getParam("frustum_skip_distance", frustum_skip_distance_);
if (nh.hasParam("frustum_column_clearing"))
nh.getParam("frustum_column_clearing", frustum_column_clearing_);
if (nh.hasParam("column_clear_min_height"))
nh.getParam("column_clear_min_height", column_clear_min_height_);
if (nh.hasParam("column_clear_max_height"))
nh.getParam("column_clear_max_height", column_clear_max_height_);
if (nh.hasParam("column_cover_distance"))
nh.getParam("column_cover_distance", column_cover_distance_);
robot::log_info("VoxelLayer frustum_skip_distance: %.3f m%s\n",
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_,
frustum_skip_distance_ >= 0.0 ? "" : " (legacy 2 * resolution)");
robot::log_info("VoxelLayer frustum_column_clearing: %s, band: [%.2f, %.2f] m, cover_distance: %.2f m%s\n",
frustum_column_clearing_ ? "true" : "false",
column_clear_min_height_,
column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_,
column_cover_distance_,
column_cover_distance_ >= 0.0 ? "" : " (auto from camera pose)");
this->matchSize();
}
@@ -618,7 +643,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
const unsigned int step = std::max(1u, observation.pixel_step_);
const double min_range = observation.min_range_;
const double max_range = observation.max_range_;
const double skip_dist = 2.0 * resolution_;
const double skip_dist =
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
const unsigned int width = std::min(depth.width, camera_info.width == 0 ? depth.width : camera_info.width);
const unsigned int height = std::min(depth.height, camera_info.height == 0 ? depth.height : camera_info.height);
updateDepthRayCache(width, height, step, fx, fy, cx, cy);
@@ -647,6 +673,65 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
const unsigned int cell_raytrace_range = cellDistance(max_range);
bool cleared_any = false;
// Column clearing: certify the beam length per pixel column and the distance
// window [cover, far] where the vertical FOV spans the whole height band.
// Outside that window a real obstacle could sit above/below the FOV, so only
// the per-pixel 3D rays may clear there.
const double band_min_h = column_clear_min_height_;
const double band_max_h =
column_clear_max_height_ >= 0.0 ? column_clear_max_height_ : max_obstacle_height_;
double cover_dist = column_cover_distance_;
double far_dist = std::numeric_limits<double>::infinity();
bool column_pass = frustum_column_clearing_ && band_max_h > band_min_h;
if (column_pass && cover_dist < 0.0)
{
const double up_half = std::atan2(cy, fy);
const double down_half = std::atan2(static_cast<double>(height) - 1.0 - cy, fy);
const double axis_elev = std::atan2(r22, std::hypot(r02, r12));
const double alpha_top = axis_elev + up_half;
const double alpha_bot = axis_elev - down_half;
const double band_top = band_max_h - oz;
const double band_bot = band_min_h - oz;
constexpr double kMinSlope = 1e-3;
cover_dist = 0.0;
if (band_top > 0.0)
{
if (alpha_top <= kMinSlope)
column_pass = false; // camera can never look up to the band top
else
cover_dist = std::max(cover_dist, band_top / std::tan(alpha_top));
}
else if (alpha_top < -kMinSlope)
{
far_dist = std::min(far_dist, band_top / std::tan(alpha_top));
}
if (band_bot < 0.0)
{
if (alpha_bot >= -kMinSlope)
column_pass = false; // camera can never look down to the band bottom
else
cover_dist = std::max(cover_dist, band_bot / std::tan(alpha_bot));
}
else if (alpha_bot > kMinSlope)
{
far_dist = std::min(far_dist, band_bot / std::tan(alpha_bot));
}
if (!column_pass)
{
robot::log_warning_throttle(
10.0, "VoxelLayer column clearing disabled: vertical FOV [%.1f, %.1f] deg at camera "
"height %.2f m never covers band [%.2f, %.2f] m\n",
alpha_bot * 180.0 / M_PI, alpha_top * 180.0 / M_PI, oz, band_min_h, band_max_h);
}
}
const unsigned int column_count = (width + step - 1) / step;
if (column_pass)
depth_column_stats_.assign(column_count, DepthColumnStat());
for (const DepthRay& local_ray : depth_ray_cache_)
{
double depth_m = 0.0;
@@ -654,6 +739,47 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
continue;
robot_geometry_msgs::Vector3 global_ray;
global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
if (column_pass)
{
const double horiz_norm = std::hypot(global_ray.x, global_ray.y);
const unsigned int col = local_ray.u / step;
if (horiz_norm > 1e-6 && col < column_count)
{
DepthColumnStat& stat = depth_column_stats_[col];
const double row_delta = std::fabs(static_cast<double>(local_ray.v) - cy);
if (stat.min_band_dist < 0.0 && row_delta < stat.best_row_delta)
{
// no in-band return yet: aim the beam along the ray nearest the
// principal row
stat.azimuth = std::atan2(global_ray.y, global_ray.x);
stat.best_row_delta = row_delta;
}
stat.has_ray = true;
if (valid)
{
// depth images store z-depth; local_ray.z is the unit ray's optical
// axis component, so depth / z is the Euclidean range
const double euclid_range = depth_m / local_ray.z;
const double pz = oz + global_ray.z * euclid_range;
if (pz >= band_min_h && pz <= band_max_h)
{
const double dist_h = horiz_norm * euclid_range;
if (stat.min_band_dist < 0.0 || dist_h < stat.min_band_dist)
{
stat.min_band_dist = dist_h;
stat.azimuth = std::atan2(global_ray.y, global_ray.x);
}
}
}
}
}
double ray_len = max_range;
if (valid && depth_m < max_range)
ray_len = std::max(0.0, depth_m - skip_dist);
@@ -661,11 +787,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
if (ray_len <= min_range)
continue;
robot_geometry_msgs::Vector3 global_ray;
global_ray.x = r00 * local_ray.x + r01 * local_ray.y + r02 * local_ray.z;
global_ray.y = r10 * local_ray.x + r11 * local_ray.y + r12 * local_ray.z;
global_ray.z = r20 * local_ray.x + r21 * local_ray.y + r22 * local_ray.z;
const double sx = ox + global_ray.x * min_range;
const double sy = oy + global_ray.y * min_range;
const double sz = oz + global_ray.z * min_range;
@@ -677,6 +798,118 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
min_x, min_y, max_x, max_y) || cleared_any;
}
if (column_pass)
{
cleared_any = clearDepthColumns(ox, oy, cover_dist, far_dist, min_range, max_range,
skip_dist, min_x, min_y, max_x, max_y) ||
cleared_any;
}
return cleared_any;
}
namespace
{
/// raytraceLine action: frees the 2D cell and wipes its whole voxel column.
class ClearFullColumn
{
public:
ClearFullColumn(unsigned char* costmap, robot_voxel_grid::VoxelGrid& voxel_grid)
: costmap_(costmap), voxel_grid_(voxel_grid)
{
}
inline void operator()(unsigned int offset)
{
costmap_[offset] = FREE_SPACE;
voxel_grid_.clearVoxelColumn(offset);
}
private:
unsigned char* costmap_;
robot_voxel_grid::VoxelGrid& voxel_grid_;
};
} // namespace
bool VoxelLayer::clipColumnSegment(double& sx, double& sy, double& ex, double& ey) const
{
// Liang-Barsky clip against the map interior; the half-resolution margin
// keeps clipped endpoints valid for worldToMap.
const double min_wx = origin_x_;
const double min_wy = origin_y_;
const double max_wx = origin_x_ + getSizeInMetersX() - 0.5 * resolution_;
const double max_wy = origin_y_ + getSizeInMetersY() - 0.5 * resolution_;
const double dx = ex - sx;
const double dy = ey - sy;
const double p[4] = {-dx, dx, -dy, dy};
const double q[4] = {sx - min_wx, max_wx - sx, sy - min_wy, max_wy - sy};
double t0 = 0.0;
double t1 = 1.0;
for (int i = 0; i < 4; ++i)
{
if (std::fabs(p[i]) < 1e-12)
{
if (q[i] < 0.0)
return false;
continue;
}
const double r = q[i] / p[i];
if (p[i] < 0.0)
t0 = std::max(t0, r);
else
t1 = std::min(t1, r);
}
if (t0 > t1)
return false;
const double bx = sx;
const double by = sy;
sx = bx + t0 * dx;
sy = by + t0 * dy;
ex = bx + t1 * dx;
ey = by + t1 * dy;
return true;
}
bool VoxelLayer::clearDepthColumns(double ox, double oy, double cover_distance,
double far_distance, double min_range, double max_range,
double skip_dist, double* min_x, double* min_y,
double* max_x, double* max_y)
{
const double start_dist = std::max(cover_distance, min_range);
bool cleared_any = false;
for (const DepthColumnStat& stat : depth_column_stats_)
{
if (!stat.has_ray)
continue;
double end_dist = stat.min_band_dist >= 0.0 ? stat.min_band_dist - skip_dist : max_range;
end_dist = std::min(std::min(end_dist, max_range), far_distance);
if (end_dist <= start_dist)
continue;
const double cos_az = std::cos(stat.azimuth);
const double sin_az = std::sin(stat.azimuth);
double sx = ox + cos_az * start_dist;
double sy = oy + sin_az * start_dist;
double ex = ox + cos_az * end_dist;
double ey = oy + sin_az * end_dist;
if (!clipColumnSegment(sx, sy, ex, ey))
continue;
unsigned int sx_m, sy_m, ex_m, ey_m;
if (!worldToMap(sx, sy, sx_m, sy_m) || !worldToMap(ex, ey, ex_m, ey_m))
continue;
ClearFullColumn clearer(costmap_, robot_voxel_grid_);
raytraceLine(clearer, sx_m, sy_m, ex_m, ey_m);
touch(sx, sy, min_x, min_y, max_x, max_y);
touch(ex, ey, min_x, min_y, max_x, max_y);
cleared_any = true;
}
return cleared_any;
}