ros_pointcloud2 转 pcl_pointcloud2 转pcl_xyz

1 篇文章 0 订阅
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/passthrough.h>

ros::Publisher pub;
//回调函数
void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  pcl::PCLPointCloud2 pcl_pc2;  //声明pcl pointcloud2
  pcl_conversions::toPCL(*cloud_msg,pcl_pc2);  //ros pointcloud2 转 pcl pointcloud2
  pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>); //声明pcl pointxyz
  pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);  //pcl pointcloud2 转 pcl pointxyz
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>); //声明pcl pointxyz
  std::cout << "PointCloud before filtering has: " << temp_cloud->points.size () << " data points." << std::endl;

  // 创建滤波器对象
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (temp_cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.0);
  pass.filter (*cloud_filtered);

  //再将滤波后的点云的数据格式转换为ROS下的数据格式发布出去
  pcl::PCLPointCloud2 pcl_pc3;         //声明pcl pointcloud2
  sensor_msgs::PointCloud2 output;     //声明ros pointcloud2
  pcl::toPCLPointCloud2(*cloud_filtered,pcl_pc2);  //pcl pointxyz 转 pcl pointcloud2
  pcl_conversions::moveFromPCL(pcl_pc2, output);   //pcl pointcloud2 转 ros pointcloud2

  pub.publish (output);
}
 
int
main (int argc, char** argv)
{
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/camera/depth/color/points", 1, cloud_cb);
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
  ros::spin ();
}

 

  • 0
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
在Python中,可以使用ROS的Python客户端库`rospy`来访问ROS消息。要将ROS PointCloud2消息换为PCL点云(PointCloudLibrary),可以使用`pcl_ros`软件包中的`pcl_conversions`模块。 以下是将ROS PointCloud2换为PCL点云并在Python中可视化的示例代码: ```python import rospy import pcl from sensor_msgs.msg import PointCloud2 from pcl import pcl_visualization def pointcloud_callback(msg): cloud = pcl.PointCloud_PointXYZRGB() pcl_conversions.msg_to_pcl(msg, cloud) visual = pcl_visualization.CloudViewing() visual.ShowMonochromeCloud(cloud) while not visual.WasStopped(): pass rospy.init_node('pcl_visualization') rospy.Subscriber('/my_pointcloud_topic', PointCloud2, pointcloud_callback) rospy.spin() ``` 在此示例中,我们订阅了名为`/my_pointcloud_topic`的ROS PointCloud2话题,并在收到消息时调用`pointcloud_callback`函数。在回调函数中,我们使用`pcl_conversions`模块将ROS消息换为PCL点云,并使用`pcl_visualization`模块将其可视化。最后,我们使用`rospy.spin()`来保持节点在运行状态,直到收到`Ctrl+C`停止节点的信号。 请注意,此代码样例假定您已经安装了`pcl_ros`和`pcl_visualization`软件包。如果您还没有安装它们,请使用以下命令进行安装: ``` sudo apt-get install ros-<ros-distro>-pcl-ros sudo apt-get install ros-<ros-distro>-pcl-visualization ``` 其中,`<ros-distro>`应替换为您正在使用的ROS发行版(例如`kinetic`或`melodic`)。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值