1.添加pcl依赖
CMakeLists.txt文件中加入:
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
rospy
sensor_msgs
std_msgs
pcl_conversions
pcl_ros
)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
package.xml文件中加入:
<build_depend>libpcl-all-dev</build_depend>
<exec_depend>libpcl-all</exec_depend>
2.查看iai_kinect2发布的数据
最开始我并不清楚iai_kinect2发布了哪些话题,我可以直接使用的。于是我运行了kinect2_bridge的launch文件。
roslaunch kinect2_bridge kinect2_bridge.launch
在运行:
rostopic list
可以看到有如下话题:
/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/rosout
/rosout_agg
于是一脸懵逼的看到这么多话题,不知道选择哪一个。
打开kinect2_bridge下面的README.md文件可以看到:
## Topics### HD Topics
The images in this topics have a FullHD resolution (1920x1080).
*Note: For correct registration of the depth image to the color image it is needed to perform a calibration.*
### Quater HD Topics
The images in this topics have a quarter FullHD resolution (960x540).
*Note: For correct registration of the depth image to the color image it is needed to perform a calibration.*
### IR/Depth Topics
This are the raw IR and depth images from the sensor (512x424).
*Note: The registration of the color image is available without a calibration. Parameters for the registration are provided by the sensor itself.*
hd为高分辨率,qhd为中分辨率,sd代表低分辨率,ir代表红外图/深度图。我所要找的就是/kinect2/sd/points发布的内容。
于是我查看/kinect2/sd/points发布的格式是什么:
rostopic type /kinect2/sd/points
xp@xp-X150M-PLUS-WS:~$ rostopic type /kinect2/sd/points
sensor_msgs/PointCloud2
得到sensor_msgs/PointCloud2信息。
3.解析sensor_msgs/PointCloud2
我们打开/opt/ros/kinetic/share/sensor_msgs/msg可以看到PoinCloud2.msg文件:
# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width# Describes the channels and their layout in the binary data blob.
PointField[] fieldsbool is_bigendian # Is this data bigendian?
uint32 point_step # Length of a point in bytes
uint32 row_step # Length of a row in bytes
uint8[] data # Actual point data, size is (row_step*height)bool is_dense # True if there are no invalid points
我们所需要查看的数据为uint8[] data,但是仔细研究可以得到,一个点是由32个data,也是32个字节进行秒速,最终通过PointField[] fields里的内容对齐解析。我没有去解析它,而是绕道使用pcl库进行转换。
4.pcl库转换PointCloud2数据
#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>
#include <string.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
pcl::visualization::CloudViewer viewer("cloud_show");
void callback(const sensor_msgs::PointCloud2& msg)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(msg,*cloud);
for (size_t i = 0; i < cloud11->points.size(); ++i)
{
//cloud->points[i].x /= 2;
//cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
//cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
}
//显示点云
viewer.showCloud(cloud);
pub_.publish(msg);
}
标注红色信息两行是最关键转换的两行,如果只想得到深度点云值,只需要将<pcl::PointXYZRGB>改为<pcl::PointXYZ>即可。