ROS机器人Diego 1#制作（九）视觉系统之使用Xtion发布点云数据

要做到机器人的SLAM自适应导航，最基本的要有激光雷达数据或者点云数据，但激光雷达目前价格太高，在淘宝上便宜的也要将近3000块，实在是太贵了，另外可替代的方法是用具有深度摄像机作为传感器发布点云数据，一般用的比较多的是微软的Kinect，或者华硕的Xtion。目前Kinect已经有2.0版本，而且二手的价格也比较便宜，但Kinect2.0支持的USB3.0接口，树莓派USB接口都是2.0的，无奈只能放弃Kinect2.0，Kinect1.0笔者曾经有过一台，影像中感觉体积太大。考虑再三后最终决定使用Xtion，赶紧到淘宝上找，发现价格不便宜，后来发现乐视电视配的第一代体感摄像头，完全是OEM的Xtion，关键是价格要比Xtion便宜好几百，果断进了一台LeTV Xtion，货到后发现装上效果还不错，先上张图：



一、安装：

1.安装OpenNI包

sudo apt-get install ros-kinetic-openni-camera
sudo apt-get install ros-kinetic-openni-launch

2.安装Xtion的新版驱动（现在买到的都是新版本的）

sudo apt-get install libopenni-sensor-primesense0

3.启动openni节点（先要在其他终端中启动roscore）

roslaunch openni_launch openni.launch

启动成功后终端应该显示如下信息



这里的警告信息可以忽略，不影响使用

4.查看摄像头的所生成的影像

rosrun image_view disparity_view image:=/camera/depth/disparity

也可以通过rviz来查看生成的影像，执行如下命令

rosrun rviz rviz

二、生成点云数据，参考了两篇文档

OpenNI本身就已经有点云数据了，这篇文章完全是看了前辈的文章，就想把这些优秀的代码整合到ROS中来

官方文档http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors

古月居的http://blog.csdn.net/hcx25909/article/details/8654684

1.源代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <XnCppWrapper.h>
#include <iostream>
#include <iomanip>
#include <vector>

using namespace xn;
using namespace std;

struct SColorPoint3D
{
float  X;
float  Y;
float  Z;
float  R;
float  G;
float  B;

SColorPoint3D( XnPoint3D pos, XnRGB24Pixel color )
{
X = pos.X;
Y = pos.Y;
Z = pos.Z;
R = (float)color.nRed / 255;
G = (float)color.nGreen / 255;
B = (float)color.nBlue / 255;
}
};

void GeneratePointCloud( DepthGenerator& rDepthGen,
const XnDepthPixel* pDepth,
const XnRGB24Pixel* pImage,
vector<SColorPoint3D>& vPointCloud )
{
// number of point is the number of 2D image pixel
DepthMetaData mDepthMD;
rDepthGen.GetMetaData( mDepthMD );
unsigned int uPointNum = mDepthMD.FullXRes() * mDepthMD.FullYRes();

// build the data structure for convert
XnPoint3D* pDepthPointSet = new XnPoint3D[ uPointNum ];
unsigned int i, j, idxShift, idx;
for( j = 0; j < mDepthMD.FullYRes(); ++j )
{
idxShift = j * mDepthMD.FullXRes();
for( i = 0; i < mDepthMD.FullXRes(); ++i )
{
idx = idxShift + i;
pDepthPointSet[idx].X = i;
pDepthPointSet[idx].Y = j;
pDepthPointSet[idx].Z = pDepth[idx];
}
}

// un-project points to real world
XnPoint3D* p3DPointSet = new XnPoint3D[ uPointNum ];
rDepthGen.ConvertProjectiveToRealWorld( uPointNum, pDepthPointSet, p3DPointSet );
delete[] pDepthPointSet;

// build point cloud
for( i = 0; i < uPointNum; ++ i )
{
// skip the depth 0 points
if( p3DPointSet[i].Z == 0 )
continue;

vPointCloud.push_back( SColorPoint3D( p3DPointSet[i], pImage[i] ) );
}
delete[] p3DPointSet;
}

int main(int argc, char** argv){
ros::init(argc, argv, "point_cloud_publisher");

ros::NodeHandle n;
ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

unsigned int num_points = 100;

int count = 0;
ros::Rate r(1.0);

/////////////////
XnStatus eResult = XN_STATUS_OK;
int i = 0;

// init
Context mContext;
eResult = mContext.Init();

DepthGenerator mDepthGenerator;
eResult = mDepthGenerator.Create(mContext);
ImageGenerator mImageGenerator;
eResult = mImageGenerator.Create(mContext);

// set output mode
XnMapOutputMode mapMode;
mapMode.nXRes = XN_VGA_X_RES;
mapMode.nYRes = XN_VGA_Y_RES;
mapMode.nFPS  = 30;
eResult = mDepthGenerator.SetMapOutputMode(mapMode);
eResult = mImageGenerator.SetMapOutputMode(mapMode);

// start generating
eResult = mContext.StartGeneratingAll();
// read data
vector<SColorPoint3D> vPointCloud;
/////////////////

while(n.ok()){

eResult = mContext.WaitNoneUpdateAll();
// get the depth map
const XnDepthPixel*  pDepthMap = mDepthGenerator.GetDepthMap();

// get the image map
const XnRGB24Pixel*  pImageMap = mImageGenerator.GetRGB24ImageMap();

// generate point cloud

d4e4
vPointCloud.clear();
GeneratePointCloud(mDepthGenerator, pDepthMap, pImageMap, vPointCloud );

// print point cloud
cout.flags(ios::left);    //Left-aligned
cout << "Point number: " << vPointCloud.size() << endl;

num_points=vPointCloud.size();

sensor_msgs::PointCloud cloud;
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = "sensor_frame";

cloud.points.resize(num_points);

//we'll also add an intensity channel to the cloud
cloud.channels.resize(3);
cloud.channels[0].name = "R";
cloud.channels[0].values.resize(num_points);
cloud.channels[1].name = "G";
cloud.channels[1].values.resize(num_points);
cloud.channels[2].name = "G";
cloud.channels[2].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 = vPointCloud[i].X;
cloud.points[i].y = vPointCloud[i].Y;
cloud.points[i].z = vPointCloud[i].Z;
cloud.channels[0].values[i] = vPointCloud[i].R;
cloud.channels[1].values[i] = vPointCloud[i].G;
cloud.channels[2].values[i] = vPointCloud[i].B;
}

cloud_pub.publish(cloud);
++count;
r.sleep();
}
return 0;
}

2.另外在包目录下的CMakeLists.txt文件中有两处修改，否则编译会出错

增加openni的引用路径

include_directories ("/usr/include/ni/")

增加新的可执行文件说明

add_executable(XtionPointCloud src/XtionPointCloud.cpp)
target_link_libraries(XtionPointCloud ${catkin_LIBRARIES})
target_link_libraries(XtionPointCloud OpenNI)

修改保存后在~/catkin_ws下执行编译命令

catkin_make

3.启动XtionPointCloud节点

rosrun diego_nav XtionPointCloud

打开另外一个终端查看发布的点云数据

rostopic echo /cloud

这时候就会看到一屏一屏的数据



树莓派处理其点云数据还是很吃力的，这个时候树莓派的系统资源使用情况：



4个CPU的使用都在50%以上

内存使用接近90%