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

add multi camera depth

Browse Source
This commit is contained in:
duongtd
2026-07-14 09:42:35 +07:00
parent a2a021c114
commit 6a9834d3a8
9 changed files with 711 additions and 1374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
include/robot_costmap_2d/voxel_layer.h
View File

@@ -91,10 +91,11 @@ private:
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
bool raytraceDepthFrustum(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y);
bool getCloudPoint(const robot_sensor_msgs::PointCloud2& cloud, unsigned int u, unsigned int v,
double& wx, double& wy, double& wz) const;
// bool raytraceDepthFrustum(double* min_x, double* min_y, double* max_x, double* max_y);
bool raytraceDepthFrustum(const robot_costmap_2d::DepthCameraObservation& observation,
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;
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);
@@ -104,8 +105,6 @@ private:
robot_voxel_grid::VoxelGrid robot_voxel_grid_;
double z_resolution_, origin_z_;
unsigned int unknown_threshold_, mark_threshold_, size_z_;
bool frustum_clearing_enabled_;
unsigned int frustum_clearing_pixel_step_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)