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

otimal deep coppy obj

Browse Source
This commit is contained in:
duongtd
2026-07-14 11:08:23 +07:00
parent 6a9834d3a8
commit bdbb03aa51
17 changed files with 702 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
plugins/voxel_layer.cpp
View File

@@ -456,6 +456,43 @@ bool VoxelLayer::readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned
return false;
}
void VoxelLayer::updateDepthRayCache(unsigned int width, unsigned int height,
unsigned int pixel_step, double fx, double fy,
double cx, double cy)
{
if (cached_depth_width_ == width && cached_depth_height_ == height &&
cached_depth_pixel_step_ == pixel_step && cached_fx_ == fx && cached_fy_ == fy &&
cached_cx_ == cx && cached_cy_ == cy)
{
return;
}
cached_depth_width_ = width;
cached_depth_height_ = height;
cached_depth_pixel_step_ = pixel_step;
cached_fx_ = fx;
cached_fy_ = fy;
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);
for (unsigned int v = 0; v < height; v += pixel_step)
{
for (unsigned int u = 0; u < width; u += pixel_step)
{
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});
}
}
}
bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz)
{
double a = wx - ox;
@@ -494,7 +531,8 @@ bool VoxelLayer::clipRaytraceEndpoint(double ox, double oy, double oz, double& w
}
bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, double wy, double wz,
double raytrace_range, double* min_x, double* min_y, double* max_x, double* max_y)
double raytrace_range, unsigned int cell_raytrace_range,
double* min_x, double* min_y, double* max_x, double* max_y)
{
double sensor_x, sensor_y, sensor_z;
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
@@ -509,7 +547,7 @@ bool VoxelLayer::clearVoxelRay(double ox, double oy, double oz, double wx, doubl
robot_voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cellDistance(raytrace_range));
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, raytrace_range, min_x, min_y, max_x, max_y);
return true;
}
@@ -583,56 +621,60 @@ bool VoxelLayer::raytraceDepthFrustum(const DepthCameraObservation& observation,
const double skip_dist = 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);
double qx = tfm.transform.rotation.x;
double qy = tfm.transform.rotation.y;
double qz = tfm.transform.rotation.z;
double qw = tfm.transform.rotation.w;
const double quaternion_norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
if (quaternion_norm <= 0.0)
return false;
qx /= quaternion_norm;
qy /= quaternion_norm;
qz /= quaternion_norm;
qw /= quaternion_norm;
const double r00 = 1.0 - 2.0 * (qy * qy + qz * qz);
const double r01 = 2.0 * (qx * qy - qz * qw);
const double r02 = 2.0 * (qx * qz + qy * qw);
const double r10 = 2.0 * (qx * qy + qz * qw);
const double r11 = 1.0 - 2.0 * (qx * qx + qz * qz);
const double r12 = 2.0 * (qy * qz - qx * qw);
const double r20 = 2.0 * (qx * qz - qy * qw);
const double r21 = 2.0 * (qy * qz + qx * qw);
const double r22 = 1.0 - 2.0 * (qx * qx + qy * qy);
const unsigned int cell_raytrace_range = cellDistance(max_range);
bool cleared_any = false;
for (unsigned int v = 0; v < height; v += step)
for (const DepthRay& local_ray : depth_ray_cache_)
{
for (unsigned int u = 0; u < width; u += step)
{
double depth_m = 0.0;
bool valid = false;
if (!readDepthMeters(depth, u, v, depth_m, valid))
continue;
double depth_m = 0.0;
bool valid = false;
if (!readDepthMeters(depth, local_ray.u, local_ray.v, depth_m, valid))
continue;
double ray_len = max_range;
if (valid && depth_m < max_range)
ray_len = std::max(0.0, depth_m - skip_dist);
double ray_len = max_range;
if (valid && depth_m < max_range)
ray_len = std::max(0.0, depth_m - skip_dist);
if (ray_len <= min_range)
continue;
if (ray_len <= min_range)
continue;
double dx = (static_cast<double>(u) - cx) / fx;
double dy = (static_cast<double>(v) - cy) / fy;
double dz = 1.0;
const double norm = std::sqrt(dx * dx + dy * dy + dz * dz);
if (norm <= 0.0)
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;
robot_geometry_msgs::Vector3 local_ray;
local_ray.x = dx / norm;
local_ray.y = dy / norm;
local_ray.z = dz / norm;
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;
const double wx = ox + global_ray.x * ray_len;
const double wy = oy + global_ray.y * ray_len;
const double wz = oz + global_ray.z * ray_len;
robot_geometry_msgs::Vector3 global_ray;
tf3::doTransform(local_ray, global_ray, tfm);
const double global_norm =
std::sqrt(global_ray.x * global_ray.x + global_ray.y * global_ray.y + global_ray.z * global_ray.z);
if (global_norm <= 0.0)
continue;
global_ray.x /= global_norm;
global_ray.y /= global_norm;
global_ray.z /= global_norm;
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;
const double wx = ox + global_ray.x * ray_len;
const double wy = oy + global_ray.y * ray_len;
const double wz = oz + global_ray.z * ray_len;
cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, min_x, min_y, max_x, max_y) || cleared_any;
}
cleared_any = clearVoxelRay(sx, sy, sz, wx, wy, wz, ray_len, cell_raytrace_range,
min_x, min_y, max_x, max_y) || cleared_any;
}
return cleared_any;
@@ -645,6 +687,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// Most update cycles do not cross a costmap cell boundary. Avoid copying and
// resetting the complete 2D/3D grids when the cell-aligned origin is unchanged.
if (cell_ox == 0 && cell_oy == 0)
return;
// compute the associated world coordinates for the origin cell
// beacuase we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
@@ -665,15 +712,20 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
const std::size_t overlap_size = static_cast<std::size_t>(cell_size_x) * cell_size_y;
rolling_costmap_scratch_.resize(overlap_size);
rolling_voxel_scratch_.resize(overlap_size);
unsigned char* local_map = rolling_costmap_scratch_.data();
unsigned int* local_voxel_map = rolling_voxel_scratch_.data();
unsigned int* voxel_map = robot_voxel_grid_.getData();
// copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
cell_size_y);
if (overlap_size > 0)
{
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0,
cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0,
cell_size_x, cell_size_x, cell_size_y);
}
// we'll reset our maps to unknown space if appropriate
resetMaps();
@@ -687,12 +739,14 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
if (overlap_size > 0)
{
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y,
size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y,
size_x_, cell_size_x, cell_size_y);
}
// make sure to clean up
delete[] local_map;
delete[] local_voxel_map;
}
// Export factory function