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 16:12:55 +07:00
parent 0e84ac53cb
commit c888af3b7c
7 changed files with 170 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
plugins/obstacle_layer.cpp
View File

@@ -133,8 +133,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
bool inf_is_valid = false, clearing=false, marking=true;
bool frustum_clearing_enabled = false;
int frustum_pixel_step = 8;
double frustum_min_range = 0.2;
double frustum_max_range = 3.0;
DepthFrustumConfig frustum_config;
robot::NodeHandle priv_nh(nh, source);
topic = loadParam(layer[source],"topic", topic);
@@ -147,10 +146,28 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
inf_is_valid = loadParam(layer[source],"inf_is_valid", false);
clearing = loadParam(layer[source],"clearing", false);
marking = loadParam(layer[source],"marking", true);
frustum_clearing_enabled = loadParam(layer, "frustum_clearing_enabled", false);
frustum_pixel_step = loadParam(layer, "frustum_clearing_pixel_step", 8);
frustum_min_range = loadParam(layer, "frustum_min_range", 0.2);
frustum_max_range = loadParam(layer, "frustum_max_range", 3.0);
// frustum params are per-source; the layer-level key is kept as a
// fallback for older YAMLs
frustum_clearing_enabled = loadParam(layer[source], "frustum_clearing_enabled",
loadParam(layer, "frustum_clearing_enabled", false));
frustum_pixel_step = loadParam(layer[source], "frustum_clearing_pixel_step",
loadParam(layer, "frustum_clearing_pixel_step", 8));
frustum_config.min_range = loadParam(layer[source], "frustum_min_range",
loadParam(layer, "frustum_min_range", 0.2));
frustum_config.max_range = loadParam(layer[source], "frustum_max_range",
loadParam(layer, "frustum_max_range", 3.0));
frustum_config.skip_distance =
loadParam(layer[source], "frustum_skip_distance", frustum_config.skip_distance);
frustum_config.column_clearing =
loadParam(layer[source], "frustum_column_clearing", frustum_config.column_clearing);
frustum_config.column_min_height =
loadParam(layer[source], "column_clear_min_height", frustum_config.column_min_height);
frustum_config.column_max_height =
loadParam(layer[source], "column_clear_max_height", frustum_config.column_max_height);
frustum_config.column_skip_distance =
loadParam(layer[source], "column_skip_distance", frustum_config.column_skip_distance);
frustum_config.column_cover_distance =
loadParam(layer[source], "column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("topic"))
priv_nh.getParam("topic", topic);
@@ -180,13 +197,34 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
frustum_pixel_step = std::max(1, frustum_pixel_step);
}
if (priv_nh.hasParam("frustum_min_range"))
priv_nh.getParam("frustum_min_range", frustum_min_range);
priv_nh.getParam("frustum_min_range", frustum_config.min_range);
if (priv_nh.hasParam("frustum_max_range"))
priv_nh.getParam("frustum_max_range", frustum_max_range);
priv_nh.getParam("frustum_max_range", frustum_config.max_range);
if (priv_nh.hasParam("frustum_skip_distance"))
priv_nh.getParam("frustum_skip_distance", frustum_config.skip_distance);
if (priv_nh.hasParam("frustum_column_clearing"))
priv_nh.getParam("frustum_column_clearing", frustum_config.column_clearing);
if (priv_nh.hasParam("column_clear_min_height"))
priv_nh.getParam("column_clear_min_height", frustum_config.column_min_height);
if (priv_nh.hasParam("column_clear_max_height"))
priv_nh.getParam("column_clear_max_height", frustum_config.column_max_height);
if (priv_nh.hasParam("column_skip_distance"))
priv_nh.getParam("column_skip_distance", frustum_config.column_skip_distance);
if (priv_nh.hasParam("column_cover_distance"))
priv_nh.getParam("column_cover_distance", frustum_config.column_cover_distance);
if (priv_nh.hasParam("frustum_depth_camera_topic"))
priv_nh.getParam("frustum_depth_camera_topic", depth_camera_data_topic_);
robot::log_info("frustum_clearing_enabled: %s, frustum_clearing_pixel_step: %d, frustum_min_range: %f, frustum_max_range: %f", frustum_clearing_enabled ? "true" : "false", frustum_pixel_step, frustum_min_range, frustum_max_range);
frustum_config.pixel_step = static_cast<unsigned int>(std::max(1, frustum_pixel_step));
robot::log_info("source %s: frustum_clearing_enabled: %s, pixel_step: %u, range: [%.2f, %.2f] m, "
"skip: %.3f m, column_clearing: %s, column_band: [%.2f, %.2f] m, "
"column_skip: %.3f m, column_cover: %.2f m\n",
source.c_str(), frustum_clearing_enabled ? "true" : "false",
frustum_config.pixel_step, frustum_config.min_range, frustum_config.max_range,
frustum_config.skip_distance, frustum_config.column_clearing ? "true" : "false",
frustum_config.column_min_height, frustum_config.column_max_height,
frustum_config.column_skip_distance, frustum_config.column_cover_distance);
double obstacle_range = 2.5;
obstacle_range = loadParam(layer[source],"obstacle_range", obstacle_range);
@@ -244,8 +282,8 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
depth_observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, frustum_pixel_step,
frustum_min_range, frustum_max_range, *tf_, global_frame_,
max_obstacle_height, obstacle_range, raytrace_range, frustum_config,
*tf_, global_frame_,
sensor_frame, transform_tolerance)));
}

95
plugins/voxel_layer.cpp
View File

@@ -98,11 +98,6 @@ 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"))
@@ -130,26 +125,6 @@ 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();
}
@@ -500,20 +475,35 @@ void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
cached_cx_ = cx;
cached_cy_ = cy;
const std::size_t rows = (height + pixel_step - 1) / pixel_step;
const std::size_t columns = (width + pixel_step - 1) / pixel_step;
depth_ray_cache_.clear();
depth_ray_cache_.reserve(rows * columns);
// Sample every pixel_step-th row/column and always include the last image
// row/column, so cells marked from border pixels stay inside the swept
// clearing fan.
std::vector<unsigned int> u_samples, v_samples;
u_samples.reserve(width / pixel_step + 2);
v_samples.reserve(height / pixel_step + 2);
for (unsigned int u = 0; u < width; u += pixel_step)
u_samples.push_back(u);
if (width > 0 && u_samples.back() != width - 1)
u_samples.push_back(width - 1);
for (unsigned int v = 0; v < height; v += pixel_step)
v_samples.push_back(v);
if (height > 0 && v_samples.back() != height - 1)
v_samples.push_back(height - 1);
cached_column_count_ = static_cast<unsigned int>(u_samples.size());
depth_ray_cache_.clear();
depth_ray_cache_.reserve(u_samples.size() * v_samples.size());
for (const unsigned int v : v_samples)
{
for (unsigned int u = 0; u < width; u += pixel_step)
for (unsigned int col = 0; col < u_samples.size(); ++col)
{
const unsigned int u = u_samples[col];
const double x = (static_cast<double>(u) - cx) / fx;
const double y = (static_cast<double>(v) - cy) / fy;
const double inverse_norm = 1.0 / std::sqrt(x * x + y * y + 1.0);
depth_ray_cache_.push_back(
DepthRay{u, v, x * inverse_norm, y * inverse_norm, inverse_norm});
DepthRay{u, v, col, x * inverse_norm, y * inverse_norm, inverse_norm});
}
}
}
@@ -640,11 +630,12 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
return false;
}
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 DepthFrustumConfig& frustum = observation.frustum_;
const unsigned int step = std::max(1u, frustum.pixel_step);
const double min_range = frustum.min_range;
const double max_range = frustum.max_range;
const double skip_dist =
frustum_skip_distance_ >= 0.0 ? frustum_skip_distance_ : 2.0 * resolution_;
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);
@@ -677,12 +668,12 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
// 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_min_h = frustum.column_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_;
frustum.column_max_height >= 0.0 ? frustum.column_max_height : max_obstacle_height_;
double cover_dist = frustum.column_cover_distance;
double far_dist = std::numeric_limits<double>::infinity();
bool column_pass = frustum_column_clearing_ && band_max_h > band_min_h;
bool column_pass = frustum.column_clearing && band_max_h > band_min_h;
if (column_pass && cover_dist < 0.0)
{
@@ -728,9 +719,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
}
}
const unsigned int column_count = (width + step - 1) / step;
if (column_pass)
depth_column_stats_.assign(column_count, DepthColumnStat());
depth_column_stats_.assign(cached_column_count_, DepthColumnStat());
for (const DepthRay& local_ray : depth_ray_cache_)
{
@@ -739,18 +729,21 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
continue;
// 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 = valid ? depth_m / local_ray.z : 0.0;
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)
if (column_pass && local_ray.col < depth_column_stats_.size())
{
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)
if (horiz_norm > 1e-6)
{
DepthColumnStat& stat = depth_column_stats_[col];
DepthColumnStat& stat = depth_column_stats_[local_ray.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)
{
@@ -763,9 +756,6 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
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)
{
@@ -781,8 +771,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
}
double ray_len = max_range;
if (valid && depth_m < max_range)
ray_len = std::max(0.0, depth_m - skip_dist);
if (valid && euclid_range < max_range)
ray_len = std::max(0.0, euclid_range - skip_dist);
if (ray_len <= min_range)
continue;
@@ -801,7 +791,8 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
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) ||
std::max(0.0, frustum.column_skip_distance),
min_x, min_y, max_x, max_y) ||
cleared_any;
}