ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion
ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion
说明:
- 介绍接入LeTV Xtion深度相机
- 机器人的SLAM自适应导航,最基本的要有激光雷达数据或者点云数据
- 但激光雷达目前价格太高,另外可替代的方法是用具有深度摄像机作为传感器发布点云数据
- 一般用的比较多的是微软的Kinect,或者华硕的Xtion。
- 目前Kinect已经有2.0版本,但Kinect2.0支持的USB3.0接口,树莓派USB接口都是2.0的
- 基于价格和体积的考虑,选择LeTV Xtion ,效果还不错。
效果图:
安装方法
- 安装OpenNI包
sudo apt-get install ros-kinetic-openni-camera
sudo apt-get install ros-kinetic-openni-launch
- 安装Xtion的新版驱动(现在买到的都是新版本的)
sudo apt-get install libopenni-sensor-primesense0
- 启动openni节点(先要在其他终端中启动roscore)
roslaunch openni_launch openni.launch
启动成功后终端应该显示如下信息
这里的警告信息可以忽略,不影响使用 ,主要是标定没做
查看摄像头的所生成的影像
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
- 源代码:
#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
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;
}
- 另外在包目录下的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
测试
- 新终端,启动roscore
roscore
- 新终端,启动XtionPointCloud节点
rosrun diego_nav XtionPointCloud
- 新终端,查看发布的点云数据
rostopic echo /cloud
- 效果图:
- 树莓派处理其点云数据还是很吃力的,这个时候树莓派的系统资源使用情况:
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号