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

107
plugins/inflation_layer.cpp
View File

@@ -58,7 +58,6 @@ InflationLayer::InflationLayer()
, inflate_unknown_(false)
, cell_inflation_radius_(0)
, cached_cell_inflation_radius_(0)
, seen_(NULL)
, cached_costs_(NULL)
, cached_distances_(NULL)
, last_min_x_(-std::numeric_limits<float>::max())
@@ -76,10 +75,8 @@ void InflationLayer::onInitialize()
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
current_ = true;
if (seen_)
delete[] seen_;
seen_ = NULL;
seen_size_ = 0;
seen_.clear();
seen_generation_ = 0;
need_reinflation_ = false;
std::string config_file_name = "inflation_layer_params.yaml";
// std::cout << "InflationLayer: " << config_file_name << std::endl;
@@ -144,10 +141,8 @@ void InflationLayer::matchSize()
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
seen_.assign(static_cast<std::size_t>(size_x) * size_y, 0);
seen_generation_ = 0;
}
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
@@ -203,26 +198,28 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
if (cell_inflation_radius_ == 0)
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
if(!inflation_cells_.empty())
robot::log_error("The inflation list must be empty at the beginning of inflation\n");
for (std::vector<CellData>& cells : inflation_cells_)
cells.clear();
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
const std::size_t map_size = static_cast<std::size_t>(size_x) * size_y;
if (seen_.size() != map_size)
{
robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
seen_.assign(map_size, 0);
seen_generation_ = 0;
}
if (seen_generation_ == std::numeric_limits<std::uint32_t>::max())
{
std::fill(seen_.begin(), seen_.end(), 0);
seen_generation_ = 1;
}
else
{
++seen_generation_;
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
@@ -238,11 +235,13 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Precomputed distance buckets preserve priority ordering without a tree lookup
// for every enqueued cell.
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
if (inflation_cells_.empty())
return;
std::vector<CellData>& obs_bin = inflation_cells_.front();
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
@@ -258,23 +257,22 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
for (std::vector<CellData>& bin : inflation_cells_)
{
for (int i = 0; i < bin->second.size(); ++i)
for (std::size_t i = 0; i < bin.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
const CellData& cell = bin[i];
unsigned int index = cell.index_;
// ignore if already visited
if (seen_[index])
if (seen_[index] == seen_generation_)
{
continue;
}
seen_[index] = true;
seen_[index] = seen_generation_;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
@@ -301,7 +299,6 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
}
}
inflation_cells_.clear();
}
/**
@@ -316,7 +313,7 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
if (seen_[index] != seen_generation_)
{
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
double distance = distanceLookup(mx, my, src_x, src_y);
@@ -325,8 +322,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
if (distance > cell_inflation_radius_)
return;
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
const unsigned int dx = std::abs(static_cast<int>(mx) - static_cast<int>(src_x));
const unsigned int dy = std::abs(static_cast<int>(my) - static_cast<int>(src_y));
const unsigned int bin_index = distance_bin_lookup_[dx * distance_lookup_size_ + dy];
inflation_cells_[bin_index].push_back(CellData(index, mx, my, src_x, src_y));
}
}
@@ -354,6 +353,38 @@ void InflationLayer::computeCaches()
}
cached_cell_inflation_radius_ = cell_inflation_radius_;
distance_lookup_size_ = cell_inflation_radius_ + 2;
distance_levels_.clear();
for (unsigned int i = 0; i < distance_lookup_size_; ++i)
{
for (unsigned int j = 0; j < distance_lookup_size_; ++j)
{
if (cached_distances_[i][j] <= cell_inflation_radius_)
distance_levels_.push_back(cached_distances_[i][j]);
}
}
std::sort(distance_levels_.begin(), distance_levels_.end());
distance_levels_.erase(
std::unique(distance_levels_.begin(), distance_levels_.end()), distance_levels_.end());
inflation_cells_.clear();
inflation_cells_.resize(distance_levels_.size());
distance_bin_lookup_.assign(
static_cast<std::size_t>(distance_lookup_size_) * distance_lookup_size_, 0);
for (unsigned int i = 0; i < distance_lookup_size_; ++i)
{
for (unsigned int j = 0; j < distance_lookup_size_; ++j)
{
const double distance = cached_distances_[i][j];
if (distance > cell_inflation_radius_)
continue;
distance_bin_lookup_[i * distance_lookup_size_ + j] =
static_cast<unsigned int>(
std::lower_bound(distance_levels_.begin(), distance_levels_.end(), distance) -
distance_levels_.begin());
}
}
}
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
@@ -367,6 +398,10 @@ void InflationLayer::computeCaches()
void InflationLayer::deleteKernels()
{
inflation_cells_.clear();
distance_levels_.clear();
distance_bin_lookup_.clear();
distance_lookup_size_ = 0;
if (cached_distances_ != NULL)
{
for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)

54
plugins/obstacle_layer.cpp
View File

@@ -269,10 +269,11 @@ void ObstacleLayer::handleImpl(const void* data,
const std::type_info& type,
const std::string& topic)
{
if(!stop_receiving_data_)
if (!enabled_ || stop_receiving_data_)
return;
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr))
{
if (type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr) )
{
const robot_sensor_msgs::DepthCameraData::ConstPtr& depth_camera_data_ptr =
*static_cast<const robot_sensor_msgs::DepthCameraData::ConstPtr*>(data);
if (!depth_camera_data_ptr)
@@ -333,7 +334,8 @@ void ObstacleLayer::handleImpl(const void* data,
// std::lock_guard<std::mutex> lock(depth_camera_data_mutex_);
// pending_depth_camera_data_ = depth_camera_data_ptr;
if(depth_observation_buffers_.empty() || callback_depth_infos_.empty()) return;
if (depth_observation_buffers_.empty() || callback_depth_infos_.empty())
return;
int size_callback_depth = static_cast<int>(callback_depth_infos_.size());
for(int i = 0; i < size_callback_depth; i++)
@@ -343,17 +345,17 @@ void ObstacleLayer::handleImpl(const void* data,
topic == callback_depth_infos_[i].topic)
{
// robot::log_error_throttle(1.0,"TEST");
depthImageCallback(depth_camera_data, buffer);
depthImageCallback(depth_camera_data_ptr, buffer);
}
}
// return;
}
else
{
if(observation_buffers_.empty() || callback_infos_.empty()) return;
}
else
{
if (observation_buffers_.empty() || callback_infos_.empty())
return;
int size_callback = static_cast<int>(callback_infos_.size());
for(int i = 0; i < size_callback; i++)
for (int i = 0; i < size_callback; i++)
{
boost::shared_ptr<ObservationBuffer>& buffer = observation_buffers_[i];
@@ -403,12 +405,6 @@ void ObstacleLayer::handleImpl(const void* data,
// << "topic check: " << callback_infos_[i].topic << std::endl << std::endl;
// }
}
}
}
else
{
robot::log_info("Stop receiving data!\n");
return;
}
}
@@ -510,12 +506,11 @@ void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& me
buffer->unlock();
}
void ObstacleLayer::depthImageCallback(const robot_sensor_msgs::DepthCameraData& message,
void ObstacleLayer::depthImageCallback(robot_sensor_msgs::DepthCameraData::ConstPtr message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
buffer->lock();
// robot::log_error_throttle(1.0, "depth data size 1: %d", (int)message.depth.data.size());
buffer->bufferDepthCamera(message);
buffer->bufferDepthCamera(std::move(message));
buffer->unlock();
}
@@ -557,6 +552,9 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
std::size_t rejected_height = 0;
std::size_t rejected_range = 0;
std::size_t rejected_bounds = 0;
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double px = *iter_x, py = *iter_y, pz = *iter_z;
@@ -564,7 +562,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the obstacle is too high or too far away from the robot we won't add it
if (pz > max_obstacle_height_)
{
robot::log_error("The point is too high\n");
++rejected_height;
continue;
}
@@ -575,7 +573,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
{
robot::log_error("The point is too far away\n");
++rejected_range;
continue;
}
@@ -583,7 +581,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
robot::log_error("Computing map coords failed\n");
++rejected_bounds;
continue;
}
@@ -591,6 +589,14 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);
}
if (rejected_height + rejected_range + rejected_bounds > 0)
{
robot::log_info_throttle(
5.0,
"ObstacleLayer filtered points: height=%zu range=%zu outside_map=%zu\n",
rejected_height, rejected_range, rejected_bounds);
}
}
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);

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