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

change topic and msg depth camera

Browse Source
This commit is contained in:
duongtd
2026-07-11 09:50:40 +07:00
parent a2a021c114
commit faa57d9c93
5 changed files with 311 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
include/robot_costmap_2d/obstacle_layer.h
View File

@@ -48,9 +48,11 @@
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/DepthCameraData.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
#include <mutex>
@@ -67,7 +69,8 @@ struct CallBackInfo
class ObstacleLayer : public CostmapLayer
{
public:
ObstacleLayer()
ObstacleLayer() :
have_depth_camera_data_(false)
{
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
}
@@ -178,6 +181,10 @@ protected:
int combination_method_;
std::vector<CallBackInfo> callback_infos_;
std::string depth_camera_topic_;
mutable std::mutex depth_camera_mutex_;
robot_sensor_msgs::DepthCameraData latest_depth_camera_data_;
bool have_depth_camera_data_;
private:
bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);

17
include/robot_costmap_2d/voxel_layer.h
View File

@@ -45,6 +45,7 @@
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_laser_geometry/laser_geometry.hpp>
#include <robot_sensor_msgs/Image.h>
#include <robot_sensor_msgs/PointCloud.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/point_cloud_conversion.h>
@@ -58,7 +59,11 @@ class VoxelLayer : public ObstacleLayer
{
public:
VoxelLayer() :
robot_voxel_grid_(0, 0, 0)
robot_voxel_grid_(0, 0, 0),
frustum_clearing_enabled_(false),
frustum_clearing_pixel_step_(8),
frustum_min_range_(0.2),
frustum_max_range_(3.0)
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D.
}
@@ -91,10 +96,9 @@ 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 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);
@@ -106,6 +110,9 @@ private:
unsigned int unknown_threshold_, mark_threshold_, size_z_;
bool frustum_clearing_enabled_;
unsigned int frustum_clearing_pixel_step_;
double frustum_min_range_;
double frustum_max_range_;
std::string frustum_depth_camera_topic_;
robot_sensor_msgs::PointCloud clearing_endpoints_;
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)