爱折腾-创客智造实验室 - ros机器人diego制作
type: channel
link: https://www.ncnynl.com/tag/ros机器人diego制作
items: ROS机器人Diego制作目录
items: ROS机器人Diego制作1-机器人硬件清单
items: ROS机器人Diego制作2-利用rosserial_arduino实现base controller
items: ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller
items: ROS机器人Diego制作4-线速度的标定
items: ROS机器人Diego制作5-角速度的标定
items: ROS机器人Diego制作6-实现两个马达独立PID调速
items: ROS机器人Diego制作7-ROS语音系统之sphinx
items: ROS机器人Diego制作8-ROS语音系统之科大讯飞语音
items: ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion

ROS机器人Diego制作目录
type: item
date: 2017-01-16T09:47:00+00:00
description: ROS机器人Diego制作目录 说明： 本专栏为转载，在原教程基础上，内容重新整理和补充 介绍从头开始打造自组装的轮式机器人 介绍如何融合ROS和自组装机器人 介绍如何进行线速度和角速度的标定或校准 介绍如何增加语音控制应用 介绍如何实现SLAM导航应用 介绍如何制作URDF模型 介绍如何增加机械臂和利用机械臂 小车图： 作者： 昵称：Diego 邮箱：mwlwlm@163.com QQ：38182368 简介：软件开发出身，爱好嵌入式开发，关注ROS机器人、人工智能、机器学习。 博客地址：http://blog.csdn.net/mwlwlm 原文： http://blog.csdn.net/mwlwlm/article/category/6449432/1 视频： http://v.youku.com/v_show/id_XMTgxNDgyMDU4OA==.html 目录： ROS机器人Diego制作目录 ROS机器人Diego制作1-机器人硬件清单 ROS机器人Diego制作2-利用rosserial_arduino实现base controller ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller ROS机器人Diego制作4-线速度的标定 ROS机器人Diego制作5-角速度的标定 ROS机器人Diego制作6-实现两个马达独立PID调速 ROS机器人Diego制作7-ROS语音系统之sphinx ROS机器人Diego制作8-ROS语音系统之科大讯飞语音 ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion ROS机器人Diego制作10-ROS视觉系统之摄像头TF数据的发布 ROS机器人Diego制作11-ROS视觉系统之点云数据转换成激光数据 ROS机器人Diego制作12-SLAM导航的配置 ROS机器人Diego制作13-整合统一的launch启动文件 ROS机器人Diego制作14-机械臂控制之arduino驱动 ROS机器人Diego制作15-机械臂控制之键盘控制 ROS机器人Diego制作16-创建机器人的urdf模型描述文件 ROS机器人Diego制作17-配置moveit，生成配置包

ROS机器人Diego制作1-机器人硬件清单
type: item
date: 2017-01-16T09:57:00+00:00
description: ROS机器人Diego制作1-机器人硬件清单 说明： 介绍教程需要的硬件 均采用开源软硬件，可以方便在淘宝购买 硬件清单： 序号 物料名称 说明 1 树莓派3 B+ 作为上位机，安装ROS机器人操作系统 2 Arduino UNO 作为下位机，实现对底盘等硬件设备的控制 3 底盘 可以自己到淘宝上选，注意一定要带码盘测速的，因为ROS要精确控制 4 电源模块 为树莓派3提供5V2.5A的电源 5 电源 9V~12V 18650充电电池 6 Arduino 电机驱动板，Arduino电机驱动板 L298P占有IO少 7 机械臂 6自由度机械臂 8 其他 线材，六角铜柱、螺丝等 ROS控制模型： 树莓派开发板： 安装kinetic ROS版本 Arduino开发板: 官网IDE下载

ROS机器人Diego制作2-利用rosserial_arduino实现base controller
type: item
date: 2017-01-16T09:58:00+00:00
description: ROS机器人Diego制作2-利用rosserial_arduino实现base controller 说明： 介绍软件包rosserial_arduino使用，此软件包用于驱动底盘 ROS的控制模型中，对底盘的驱动+马达的PID调节 = base controller 参考：http://wiki.ros.org/rosserial_arduino/Tutorials rosserial_arduino的安装 详细的安装和使用参考，ROS与Arduino专栏。 下载：进入你的workspace目录下的src目录，catkin_ws是workspace cd ~/catkin_ws/src git clone https://github.com/ros-drivers/rosserial.git 编译：在workspace目录编译 cd <catkin_ws> catkin_make 在Arduino的/libraries目录下生产ros_lib包 一般情况Arduino的libraries目录是/home/username/sketchbook/libraries 其中username代表的是登录linux的用户名 cd /home/username/sketchbook/libraries rosrun rosserial_arduino make_libraries.py 如果提示找不到rosserial_arduino make_libraries.py，则需要执行如下代码： rosserial_arduino make_libraries.py 这时候libraries目录下就会有一个ros_lib的目录，重启Arduino IDE既可以看到Ros_lib包了 也可以copy出来在其他电脑上使用 Arduino上实现ROS Node，订阅Twist msg 首先需要包含ros的头文件： #include <PID_v1.h> #include <ArduinoHardware.h> #include <ros.h> #include <geometry_msgs/Twist.h> #include <ros/time.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> 声明node句柄 ros::NodeHandle nh; 定义收到Twist msg后的处理函数 void motor_cb(const geometry_msgs::Twist& vel) { linear = vel.linear.x * 100; //ROS中的单位是m/s;这里换算成cm的单位，在Diego机器人中使用CM作为单位 angular = vel.angular.z; } //这里先暂时订阅Turtle1 package的Twist消息，后面根据自己的需要可以修改 ros::Subscriber<geometry_msgs::Twist> sub("/turtle1/cmd_vel", motor_cb); 在loop中执行 nh.spinOnce(); 到这里Arduino已经可以作为一个Node的节点接收上位机的Twist msg了 底盘驱动及PID控制 引脚定义,底盘马达驱动采用了L298P模块 #define E_left 5 //L298P直流电机驱动板的左轮电机使能端口连接到数字接口5 #define M_left 4 //L298P直流电机驱动板的左轮电机转向端口连接到数字接口4 #define E_right 6 //连接小车右轮电机的使能端口到数字接口6 #define M_right 7 //连接小车右轮电机的转向端口到数字接口7 电机马达码盘中断定义 #define Pin_left 2 //外部中断0,左轮 #define Pin_right 3 //外部中断1,右轮 底盘前进-后退-停止控制 void advance()//前进 { digitalWrite(M_left, HIGH); analogWrite(E_left, val_left); digitalWrite(M_right, HIGH); analogWrite(E_right, val_right); } void back()//后退 { digitalWrite(M_left, LOW); analogWrite(E_left, val_left); digitalWrite(M_right, LOW); analogWrite(E_right, val_right); } void Stop()//停止 { digitalWrite(E_r…

ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller
type: item
date: 2017-01-16T09:58:00+00:00
description: ROS机器人Diego制作3-利用ros_arduino_bridge实现base controller 说明： 介绍软件包ros_arduino_bridge使用，此软件包用于驱动底盘 ROS的控制模型中，对底盘的驱动+马达的PID调节 = base controller 参考：http://wiki.ros.org/ros_arduino_bridge ros_arduino_bridge封装了通过串口于底盘控制器Arduino的通信，并提供了一个标准的base controller，所以对arduino资源的占用非常小 arduino只是作为一个单纯的硬件控制器来使用，而所有的运算逻辑都放在上位机进行，通过串口指令的方式控制Arduino程序的运行。 在Arduino UNO编译一个完整的ros_arduino_bridge后的资源占用情况如下图： 从上图可以看出占用资源非常上，用户还可以在Arduino UNO开发更多的应用，接更多的传感器 ros_arduino_bridge的安装 详细的安装和使用说明，参考ROS与Arduino-ros_arduino_bridge-功能包的使用 下载：进入你的workspace目录下的src目录，catkin_ws是workspace cd ~/catkin_ws/src git clone https://github.com/hbrobotics/ros_arduino_bridge.git 编译：在workspace目录编译 cd <catkin_ws> catkin_make 使用：拷贝Arduino库文件到相应的Arduino IDE的libraries目录 $ cd SKETCHBOOK_PATH//Arduino IDE的库文件目录 $ \cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge -T ROSArduinoBridge 这时候可以把ROSArduinoBridge拷贝到其他windows， Mac电脑的Arduino IDE环境下使用，重启后既可以用 开发Arduino代码 从Example中打开ROSArduinoBridge的示例代码，另存为自己喜欢的项目名称，我们只需要根据自己的需求修改示例代码即可 示例代码文件介绍 ROSArduinoBridge.ino 主程序 commands.h 串口命令的预定义 diff_controller.h PID控制代码 encoder_driver.h 编码器，这里只是针对了Arduino UNO，使用了中断接口D2,D3,和模拟接口A4，A5；所以电机编码器的输出接线需要按照此规则接线，另外要注意编码器要有两路输出 左侧电机的编码输出接D2,D3；右侧电机的编码输出接A4,A5 encoder_driver.ino 编码器的实现代码 motor_driver.h 马达驱动的接口定义，用不动的马达驱动板都要实现此文件定义的三个函数 motor_driver.ino马达驱动实现代码，根据预定义选择不同的驱动板库，在这里我使用了L298P，所以需要自己实现一个新的驱动库，后面会介绍 sensors.h传感器的实现文件 servos.h舵机的实现文件 修改ROSArduinoBridge.ino 为了满足我们控制的需要需要修改的是ROSArduinoBridge.ino文件，主要修改的点如下： 启用Base Controller #define USE_BASE // Enable the base controller code启用base controller //#undef USE_BASE // Disable the base controller code 马达控制板定义 /* Define the motor controller and encoder library you are using */ #ifdef USE_BASE /* The Pololu VNH5019 dual motor driver shield */ //#define POLOLU_VNH5019 /* The L298P dual motor driver shield，这里我们使用我们自己写的L298P库 */ #define L298P /* The Pololu MC33926 dual motor driver shield */ //#define POLOLU_MC33926 /* The RoboGaia encoder shield */ //#define ROBOGAIA /* Encoders dir…

ROS机器人Diego制作4-线速度的标定
type: item
date: 2017-01-16T09:58:00+00:00
description: ROS机器人Diego制作4-线速度的标定 说明： 介绍线速度的标定方法，校准直线行走的偏差. 让机器人动起来容易，但要精确控制是比较难的，这与控制算法、硬件都有比较大的相关性。 对于轮式机器人，base controller代码写好，机器人动起来后。 首先做的就是需要对线速度，和角速度进行标定，以保证机器人可以按照指令精确的的形式 线速度的标定方法： 线速度的标定，就是设置一个1m的距离，代码控制让机器人行驶1m，看是否刚好是1m，误差在可接受的范围内即可 如下是进行标定的python代码： #!/usr/bin/env python import rospy from geometry_msgs.msg import Twist, Point from math import copysign, sqrt, pow import tf class CalibrateLinear(): def __init__(self): # Give the node a name rospy.init_node('calibrate_linear', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) # Set the distance to travel self.test_distance = 1.0 # meters self.speed = 1.0 # meters per second self.tolerance = 0.01 # meters self.odom_linear_scale_correction = 1.0 self.start_test = True # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") self.position = Point() # Get the starting position from the tf transform between the odom and base frames self.position = self.get_position() x_start = self.position.x y_start = self.position.y move_cmd = Twist() while not rospy.is_shutdown(): # Stop the robot by default move_cmd = Twist() if self.start_test: # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() # Comput…

ROS机器人Diego制作5-角速度的标定
type: item
date: 2017-01-16T09:59:00+00:00
description: ROS机器人Diego制作5-角速度的标定 说明： 介绍角速度标定，校准旋转角度偏差 角速度标定方法： 角速度标定，即控制机器人人转动固定的角度，看机器人是否按照控制指令完成，在这里我们让机器人转360度 python代码如下： #!/usr/bin/env python import rospy from geometry_msgs.msg import Twist, Quaternion from nav_msgs.msg import Odometry import tf from math import radians, copysign from transform_utils import quat_to_angle, normalize_angle import PyKDL from math import pi class CalibrateAngular(): def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = 2*pi #这里注意，在ROS中使用的弧度，不能直接写360 self.speed = 0.1 # radians per second self.tolerance = 1 # degrees converted to radians self.odom_angular_scale_correction = 1 self.start_test = True # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is usually base_link or base_footprint self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() rospy.loginfo("self.odom_angle: "+str(self.odom_angle)) last_angle = self.odom_angle turn_angle = 0 self.test_angle *= reverse error = self.test_angle - turn_angle rospy.loginfo("errir: "+str(error)) # Alternate directions between tests reverse = -reverse while abs(error) > self.toler…

ROS机器人Diego制作6-实现两个马达独立PID调速
type: item
date: 2017-01-16T09:59:00+00:00
description: ROS机器人Diego制作6-实现两个马达独立PID调速 说明： ros_arduino_bridge的base controller中对两个电机的PID控制是用的一套PID参数 但实际的使用中，由于马达的特性，地形，或者机器人的载重的平衡性的诸多问题使机器人不能按照预定轨迹行驶 最简单的就是机器人是否能够走直线，这就需要对两个电机分别进行PID调速 本文中将介绍如何对ros_arduino_bridge进行修改，以使支持对两路电机用不同的PID参数调速 修改diff_controller.h文件 增加左右两个马达的PID控制变量： /* PID Parameters */ int Kp = 20; int Kd = 12; int Ki = 0; int Ko = 50; int left_Kp=Kp; int left_Kd=Kd; int left_Ki=Ki; int left_Ko=Ko; int right_Kp=Kp; int right_Kd=Kd; int right_Ki=Ki; int right_Ko=Ko; 分别定义左右两个马达的dorightPID()和doleftPID()函数 /* PID routine to compute the next motor commands */ void dorightID(SetPointInfo * p) { long Perror; long output; int input; //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); input = p->Encoder - p->PrevEnc; Perror = p->TargetTicksPerFrame - input; /* * Avoid derivative kick and allow tuning changes, * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; // p->PrevErr = Perror; output = (right_Kp * Perror - right_Kd * (input - p->PrevInput) + p->ITerm) / right_Ko; p->PrevEnc = p->Encoder; output += p->output; // Accumulate Integral error *or* Limit output. // Stop accumulating when output saturates if (output >= MAX_PWM) output = MAX_PWM; else if (output <= -MAX_PWM) output = -MAX_PWM; else /* * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ */ p->ITerm += Ki * Perror; p->output = output; p->PrevInput = input; } /* PID routine to compute the next motor commands */ void doleftPID(SetPointInfo * p) { long Perror; long output; int input; //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); input = p->Encoder - p->PrevEnc; Perror =p->TargetTicksPerFrame + input; /* * Avoid derivative kick and allow tuning changes, * see http://brettbeauregard.com/blog/20…

ROS机器人Diego制作7-ROS语音系统之sphinx
type: item
date: 2017-01-16T09:59:00+00:00
description: ROS机器人Diego制作7-ROS语音系统之sphinx 说明： 介绍如何接入语音控制系统sphinx 参考其他sphinx接入教程 turtlebot入门-语音控制 http://blog.csdn.net/mwlwlm/article/details/52750364 sphinx接入方法 在ROS kinetic这个版本中是没有安装sphinx的，需要手动安装 安装如下依赖包: sudo apt-get install ros-kinetic-audio-common sudo apt-get install libasound2 sudo apt-get install gstreamer0.10-* sudo apt-get install python-gst0.10 安装libsphinxbase1 下载的网站：https://packages.debian.org/jessie/libsphinxbase1 由于Diego使用的是树莓派平台，所以请下载armhf版本的 下载完后执行 sudo dpkg -i libsphinxbase1_0.8-6_amdhf.deb 安装libpocketsphinx1 下载的网站：https://packages.debian.org/jessie/libpocketsphinx1 也下载armhf版本，下载完成后后执行 sudo dpkg -i libpocketsphinx1_0.8-5_amdhf.deb 安装gstreamer0.10-pocketsphinx 下载的网站：https://packages.debian.org/jessie/gstreamer0.10-pocketsphinx 同样下载armhf版本，下载完后执行 sudo dpkg -i gstreamer0.10-pocketsphinx_0.8-5_amdhf.deb 安装pocketsphinx 进入工作目录,克隆git目录 cd ~/catkin_ws/src Git clone https://github.com/mikeferguson/pocketsphinx 下载英文语音包pocketsphinx-hmm-en-tidigits (0.8-5) 下载的网站：https://packages.debian.org/jessie/pocketsphinx-hmm-en-tidigits 在包pocketsphinx下面建一个model目录，存放语音模型文件 cd ~/catkin_ws/src/pocketsphinx mkdir model 将下载好的语音文件，解压后，将其中的model文件下的所有文件拷贝到~/catkin_ws/src/pocketsphinx/model下 在~/catkin_ws/src/pocketsphinx目录下新建launch文件夹，创建diego_voice_test.launch文件 cd ~/catkin_ws/src/pocketsphinx mkdir launch vi diego_voice_test.launch diego_voice_test.launch文件内容如下 <launch> <node name="recognizer" pkg="pocketsphinx" type="recognizer.py" output="screen"> </node> </launch> 修改recognizer.py文件,在def init(self):函数中增加hmm参数的读取 def __init__(self): # Start node rospy.init_node("recognizer") self._device_name_param = "~mic_name" # Find the name of your microphone by typing pacmd list-sources in the terminal self._lm_param = "~lm" self._dic_param = "~dict" self._hmm_param = "~hmm" #增加hmm参数 # Configure mics with gstreamer launch config if rospy.has_param(self._device_name_param): self.device_name = rospy.get_param(self._device_name_param) self.device_index = self.pulse_index_from_name(self.device_name) self.l…

ROS机器人Diego制作8-ROS语音系统之科大讯飞语音
type: item
date: 2017-01-16T10:00:00+00:00
description: ROS机器人Diego制作8-ROS语音系统之科大讯飞语音 说明： 介绍如何接入科大讯飞语音 非常感谢ljthink提供的讯飞语音的树莓派SDK下载地址 也提供本站的下载地址：点击树莓派SDK下载地址 整合讯飞语音非常简单，网友已开发了相应的ROS讯飞包(原教程此为无效链接待修正)，按照文中的步骤执行即可。 也提供本站集成方法：ROS科大讯飞语音包介绍和下载 注意的事项及使用体验： 讯飞在为每个开发者的每一个应用提供一个appid，而且这个id是固化在libmsc.so文件中 所以在程序中在给appid赋值时要与申请的libmsc.so中的id一致，如果不一致的话编译不会出错 但是在语音识别的时候报错，但不影响语音合成的功能。 也许是树莓派本身性能的问题，在使用语音合成的时候合成出来的声音会有断断续续的现象。 语音识别有时候会出现乱码的现象，不知道是什么原因。 使用语音合成，需要安装mplayer包。 通过控制台发送xfwords主题的节点 python代码: #!/usr/bin/env python import rospy from std_msgs.msg import String if __name__=="__main__": pub = rospy.Publisher('xfwords', String, queue_size = 0) rospy.init_node('xfei_xfwords_publish', anonymous=True) rate = rospy.Rate(1) # 10hz try: while not rospy.is_shutdown(): input_str = raw_input("Please input your words:\n") pub.publish(input_str) rate.sleep() except rospy.ROSInterruptException: pass 将此节点放在xfer_asr/scripts/目录下 执行: rosrun xfei_asr xfei_xfwords_publish.py 既可以启动该节点，会把控制台输入的汉字以xfwords主题的形式发布出去，配合tts_subscribe_speak使用

ROS机器人Diego制作9-ROS视觉系统之LeTV Xtion
type: item
date: 2017-01-16T10:00:00+00:00
description: 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]; } } // …