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

add pcl

Browse Source
This commit is contained in:
duongtd
2026-07-21 14:53:10 +07:00
parent bb461b263d
commit 88cf78d66e
5 changed files with 72 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/depth_local_costmap_noetic_test_node.cpp
View File

@@ -26,6 +26,11 @@
#include <tf2_ros/transform_listener.h>
#include <tf3/buffer_core.h>
#include <data_convert/pcl_convert.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <atomic>
#include <cmath>
#include <exception>
@@ -265,6 +270,10 @@ public:
: local_costmap_(local_costmap)
{
private_nh.param("depth_cloud_topic", depth_cloud_topic_, std::string("/camera/depth/points_proc"));
private_nh.param("enable_depth_cloud_voxel_filter", enable_voxel_filter_, true);
private_nh.param("depth_cloud_voxel_leaf_size", voxel_leaf_size_, 0.10);
voxel_leaf_size_ = std::max(0.0, voxel_leaf_size_);
depth_cloud_sub_ =
private_nh.subscribe(depth_cloud_topic_, 1, &DepthCloudFeeder::depthCloudCallback, this);
}
@@ -276,16 +285,48 @@ private:
return;
const robot_sensor_msgs::PointCloud2 robot_cloud = toRobotPointCloud2(*msg);
feedVoxelLayers(local_costmap_, robot_cloud, depth_cloud_topic_);
const robot_sensor_msgs::PointCloud2 marking_cloud = filterMarkingCloud(robot_cloud);
feedVoxelLayers(local_costmap_, marking_cloud, depth_cloud_topic_);
ROS_INFO_THROTTLE(5.0, "Fed depth cloud to robot_costmap_2d: %ux%u frame=%s",
ROS_INFO_THROTTLE(5.0,
"Fed depth cloud to robot_costmap_2d: in=%ux%u out=%ux%u leaf=%.3f frame=%s",
msg->width,
msg->height,
marking_cloud.width,
marking_cloud.height,
enable_voxel_filter_ ? voxel_leaf_size_ : 0.0,
msg->header.frame_id.c_str());
}
robot_sensor_msgs::PointCloud2 filterMarkingCloud(const robot_sensor_msgs::PointCloud2& cloud) const
{
if (!enable_voxel_filter_ || voxel_leaf_size_ <= 0.0 || cloud.width == 0 || cloud.height == 0)
return cloud;
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromROBOTMsg(cloud, pcl_cloud);
if (pcl_cloud.empty())
return cloud;
pcl::VoxelGrid<pcl::PointXYZ> voxel;
voxel.setInputCloud(pcl_cloud.makeShared());
voxel.setLeafSize(static_cast<float>(voxel_leaf_size_),
static_cast<float>(voxel_leaf_size_),
static_cast<float>(voxel_leaf_size_));
pcl::PointCloud<pcl::PointXYZ> filtered;
voxel.filter(filtered);
robot_sensor_msgs::PointCloud2 output;
pcl::toROBOTMsg(filtered, output);
output.header = cloud.header;
return output;
}
robot_costmap_2d::Costmap2DROBOT& local_costmap_;
std::string depth_cloud_topic_;
bool enable_voxel_filter_{true};
double voxel_leaf_size_{0.10};
ros::Subscriber depth_cloud_sub_;
};