void CClusterHelper::PreProcessAndGetROI(pcl::PointCloud<PointT>::Ptr cloud_in, pcl::PointCloud<PointT>::Ptr selected_cloud) {
if (cloud_in->points.empty()) {
RCLCPP_ERROR(rclcpp::get_logger("CClusterHelper"), "PreProcessAndGetROI Input point cloud is empty!");
return;
}
// 找平并去除地面
pcl::PointCloud<PointT>::Ptr ground_removed_cloud(new pcl::PointCloud<PointT>());
removeGround(cloud_in, ground_removed_cloud);
pcl::PointCloud<PointT>::Ptr selected_cloud(new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr selected_cloud1(new pcl::PointCloud<PointT>());
// 更新 selected_cloud
*selected_cloud = *ground_removed_cloud;
// 滤波点云
filterCloud(selected_cloud,selected_cloud1); // 对选择的点云进行滤波
*selected_cloud = *selected_cloud1;
sensor_msgs::msg::PointCloud2 pcl3d_msg;
pcl::toROSMsg(*selected_cloud1, pcl3d_msg);
pcl3d_msg.header.frame_id = "laser_link";
pcl3d_msg.header.stamp = this->now();
cloud_publisher_->publish(pcl3d_msg);
RCLCPP_INFO(rclcpp::get_logger("CClusterHelper"),"selected cloud size:%ld",selected_cloud->points.size());
}
pcl::toROSMsg(*filtered_cloud, pcl3d_msg)
只是将 PCL 点云数据转换为 ROS 消息格式,并没有自动复制header.frame_id
和header.stamp
第二种写法:
sensor_msgs::msg::PointCloud2 pcl3d_msg;
pcl3d_msg.header.frame_id = "laser_link";
pcl3d_msg.header.stamp = cluster_node_->now();
pcl::toROSMsg(*selected_cloud, pcl3d_msg);
cloud_publisher_->publish(pcl3d_msg);
/*像这种的先给消息fram_id和时间戳,后转换即有可能被selected_cloud覆盖掉pcl3d_msg的id以及时间戳*/
虽然第二种写法可以可以在topic echo 中显示出数据,但是在rviz中是无法显示出点云的,这是因为被selected_cloud覆盖掉pcl3d_msg的id以及时间戳,rviz没有同步时间戳以及fram_id