ROS点云数据显示

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/wangbaodong070411209/article/details/88534687

• 创建工程
  首先，我们创建ROS Package，假设，我们创建的名为wb_serial。接下来打开src下的main.cpp文件，添加节点代码和发布点云数据代码。这里我们截取了两段：
  第一段：创建节点

//初始化节点 
ros::init(argc, argv, "lbwb_ladar"); 
//声明节点句柄 
ros::NodeHandle nh; 
// 发布话题
//ros::Publisher read_pub = nh.advertise<std_msgs::String>("lbwb_data", 1000); 
ros::Publisher lbwb_pub_0 = nh.advertise<sensor_msgs::PointCloud>("lbwb_pointer", 1000);

第二段：发布点云数据

// 点云
if (data->detecte_objs != NULL)
{
	
	printf("num_detecte_obj: %d\n",data->num_detecte_obj);
	for (i = 0; i< data->num_detecte_obj; i++)
	{
		printf("speed: %f\n",data->detecte_objs[i].speed);
		printf("peak: %d \n",data->detecte_objs[i].peakval);
		printf("x: %f \n",data->detecte_objs[i].x);
		printf("y: %f \n",data->detecte_objs[i].y);
		printf(" \n\n");
	}
	// 发布点云数据
	num_points = data->num_detecte_obj;
	sensor_msgs::PointCloud cloud;
	cloud.header.stamp = ros::Time::now();
	cloud.header.frame_id = "sensor_pointer";
	cloud.points.resize(num_points);
	// 
	cloud.channels.resize(2);
	cloud.channels[0].name = "peakval";
	cloud.channels[0].values.resize(num_points);
	cloud.channels[1].name = "speed";
	cloud.channels[1].values.resize(num_points);						
	// generate some fake data for our point cloud
	for(unsigned int i = 0; i < num_points; ++i){
		cloud.points[i].x = data->detecte_objs[i].x;
		cloud.points[i].y = data->detecte_objs[i].y;
		cloud.points[i].z = 5;
		cloud.channels[0].values[i] = data->detecte_objs[i].peakval;
		cloud.channels[1].values[i] = data->detecte_objs[i].speed;
	}
	lbwb_pub_0.publish(cloud);
}

然后，我们创建一个launch文件：

<?xml version="1.0"?>
<launch>
	<node name ="lbwb_ladar" pkg="wb_serial" type="lbwb_serial" output="screen"/>
</launch> 

至此，我们已经可以做下面的实验了，也有一些通用的操作，比如创建rviz文件。

• 编译
  catkin_make
  如果没问题的话，我们就打开一个命令窗口输入：

~/catkin_ws$ roslaunch wb_serial lbwb_serial.launch

接下来，我们就可以在新的窗口运行一些ROS自带的工具，比如：rosnode,rosmsg, rostopic等等。
这里，我们直接通过Rviz查看点云数据，输入如下命令：

rosrun rviz rviz
或者
rosrun rviz rviz -d `rospack find wb_serial`/config/example9.rviz

打开Rviz界面之后，我们需要添加我们的点云数据，如下图所示：
然后，我们需要设置几个参数，为了是传感器采集的数据在世界坐标系显示，首先，要知道以什么为基准。所以，在Global Options里将Fixed Frame填写我们自己的frame_id，也就是cloud.header.frame_id = “sensor_pointer”;，其次，我们需要修改PointCloud里的Topic为我们发布的主题， nh.advertise<sensor_msgs::PointCloud>(“lbwb_pointer”, 1000);。执行完上面操作，我们就能看到传感器的点云数据在Rviz的三维空间中显示。
今天的分享就到这里，感觉受益的同学们请点个赞。