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

24
include/robot_costmap_2d/voxel_layer.h
View File

@@ -96,9 +96,12 @@ private:
double* min_x, double* min_y, double* max_x, double* max_y);
bool readDepthMeters(const robot_sensor_msgs::Image& depth, unsigned int u, unsigned int v,
double& depth_m, bool& is_valid) const;
void updateDepthRayCache(unsigned int width, unsigned int height, unsigned int pixel_step,
double fx, double fy, double cx, double cy);
bool clipRaytraceEndpoint(double ox, double oy, double oz, double& wx, double& wy, double& wz);
bool 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);
bool publish_voxel_;
@@ -106,6 +109,25 @@ private:
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
std::vector<unsigned char> rolling_costmap_scratch_;
std::vector<unsigned int> rolling_voxel_scratch_;
struct DepthRay
{
unsigned int u;
unsigned int v;
double x;
double y;
double z;
};
std::vector<DepthRay> depth_ray_cache_;
unsigned int cached_depth_width_ = 0;
unsigned int cached_depth_height_ = 0;
unsigned int cached_depth_pixel_step_ = 0;
double cached_fx_ = 0.0;
double cached_fy_ = 0.0;
double cached_cx_ = 0.0;
double cached_cy_ = 0.0;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
{