将PCD点云文件传入Vrep的一种方法

写在前面开源思路外部程序避坑外部程序代码 Vrep脚本注Vrep脚本代码最终效果图：

写在前面

最近遇到一个项目，需要把PCD点云文件中的点云导入到Vrep中进行仿真，然而Vrep中并没有读取PCD文件的接口，但是Vrep支持添加点云进入场景进行仿真。

开源

Github链接：https://github.com/wangrui449966/Vrep_PCD C++的pcl环境和RemotApi环境需要自行配置

思路

通过外部的程序，读取PCD文件，得到点云后，通过RemotApi发送到Vrep中并增加这些点云。 后来发现增加点云的函数(在regular Api中)

number pointCloudHandle=simAddPointCloud(number pageMask,number layerMask,number objectHandle,number options,number pointSize,table pointCoordinates,table_12 defaultColors=nil,table pointColors=nil,table pointNormals=nil)

在RemoteApi中并没有相应的函数（如果有的话，函数名应该为simxAddPointCloud，可能以后的Vrep版本中会添加此函数），那么只能考虑在外部程序中将点云的坐标通过Vrep的stringSignal来发送给Vrep，再在Vrep的脚本中手动写读取坐标，并增加点云的方式来实现。

外部程序

避坑

1.外部程序我选择了用C++来写，因为读取PCD文件的库（pcl库）貌似C++环境配置起来更加方便（后来我发现我错了），也许python的pcl库会更方便。 2.pcl库不但是读取PCD文件这么简单，我觉得这个库的最大意义还是在于显示点云，它可以3D显示出点云，而且可以鼠标拖动来切换视角，我以rabbit.pcd为例，放一张pcl库打开后的图像： 后来我发现pcd文件是一个ascii编码的文件，直接用记事本打开，我们来看一下内容 实际上我们需要的数据就是下面这些点的坐标（x y z，跟文件头中的FIELDS指示的顺序一样，如果要深入了解一下PCD文件头的所有字段，可以百度，资料还是挺多的~）。如果只是要得到坐标信息，用庞大的pcl库显得有些浪费且没有必要花那么多精力去配置pcl的环境，完全可以自己写一段代码来读入这些坐标（我太懒啦，以后有时间的话自己写一下这段代码，C和python实现都开源一下）。

外部程序代码

在Github中开源的代码注释已经很详细了，我在这里就写一下发送点云时的重点吧，因为有些大佬懒得去Github看代码。

//这里就是初始化了一下跟Vrep的通信，获取一下ClientID c_wr_vrep.vrep_init(); float* buff; //这里cloud->points.size()是点云里的点的个数 //每个点云的坐标是xyz（三个float型数据），所以要开这么多的内存 buff = (float*)malloc(sizeof(float) * cloud->points.size() * 3); if (buff == NULL) { cout << "malloc failed???" << endl; } else { cout << "malloc success" << endl; int i; //cloud类实体是pcl库读出的点云，我们将它提取到buff里方便发送到Vrep for (i = 0; i < cloud->points.size();i++) { buff[i * 3 + 0] = (float)cloud->points[i].x; buff[i * 3 + 1] = (float)cloud->points[i].y; buff[i * 3 + 2] = (float)cloud->points[i].z; } //这时buff中的内容已经是x1 y1 z1 x2 y2 z2 x3 y3 z3.........（数字是点的index） //c_wr_vrep.set_point_cloud这个函数内容见下一个代码段 c_wr_vrep.set_point_cloud(cloud->points.size(), buff); free(buff);//释放一下内存 cout << "free ed" << endl;

c_wr_vrep.set_point_cloud：

//size:points'num buff:x,y,z,x,y,z...... void c_wr_vrep_t::set_point_cloud(unsigned long size, float* buff) { if (this->inited == false)//这里检查是不是已经连接成功 { return; } //创建这个StringSignal并且将buff发送到这个StringSignal里，这样Vrep脚本就可以通过读这个StringSignal来获取点云了 simxSetStringSignal(this->clientID, "point_cloud_cpp_signal", (simxUChar*)buff, size * sizeof(float) * 3, simx_opmode_oneshot); //在Vrep的状态栏里显示一下，表示外部程序已经发送了，这个时候点云应该会在Vrep里显示出来才对 simxAddStatusbarMessage(this->clientID, "(from C++)\"point_cloud_cpp_signal\"send\r\n", simx_opmode_oneshot); }

c_wr_vrep：这是一个类实体

typedef class { public: bool inited;//是否已经连接 int clientID;//获取到clientID int vrep_init();//连接 void set_point_cloud(unsigned long size, float* buff);//发送点云 }c_wr_vrep_t; extern c_wr_vrep_t c_wr_vrep;

Vrep脚本

注

Vrep脚本的任务是读取外部程序创建的StringSignal得到点云坐标，并增加到点云中。 Vrep中我放置了一个空的Point_cloud，待会儿点云就增加到这个Point_cloud里，Pioneer_p3dx就是加进来写脚本的。

Vrep脚本代码

初始化区：

if (sim_call_type==sim_childscriptcall_initialization) then motorLeft=simGetObjectHandle("Pioneer_p3dx_leftMotor") motorRight=simGetObjectHandle("Pioneer_p3dx_rightMotor") --WR inited=0--用来记录是否已经收到点云，收到并增加点云后置1，防止重复增加点云消耗性能 simExtRemoteApiStart(66668)--Vrep-C++ 打开供外部RemoteApi连接的端口 simClearStringSignal("point_cloud_cpp_signal");--清除一下SignalString，因为停止仿真后SignalString并不会清除，其实严谨一点，应该再放置在清除区（sim_childscriptcall_cleanup），但为了方便，毕竟我太懒了 --wr end

正常运行区：

if (sim_call_type==sim_childscriptcall_actuation) then --WR buff_raw=simGetStringSignal("point_cloud_cpp_signal")--得到StringSignal，这个时候里面还是一个一个字节的内存值 --其实这里我还考虑了一下windows默认是小端模式，会不会Vrep不是小端模式，需要每个float数据的字节顺序特别处理一下，实际上我多虑了 if buff_raw and inited==0 then --如果buff_raw不为空且还没有添加点云 buff_float=simUnpackFloatTable(buff_raw) --将String变成一个个float inited=1--标记一下已经添加点云，防止下个仿真周期再次添加点云消耗性能 simAddStatusbarMessage("draw_cloud")--状态栏通知一下，要开始加点云了 if true then local pointSize=2 --点云的每个点的大小（像素） local pts={}--用来存放点云中每个点的坐标 local colors={}--用来存放点云中每个点的颜色（为了好看一些） for i=0,#buff_float/3-1,1 do --在这个循环里 读取坐标，并生成颜色 --通过坐标生成颜色，这里怎么生成都无所谓 colors[i*3+1]=(math.sin(buff_float[i*3+1]*0.5)+1)/2*80+127 colors[i*3+2]=(math.sin(buff_float[i*3+2]*0.5)+1)/2*80+127 colors[i*3+3]=(math.sin(buff_float[i*3+3]*0.5)+1)/2*80+127 --读取坐标 pts[i*3+1]=buff_float[i*3+1] pts[i*3+2]=buff_float[i*3+2] pts[i*3+3]=buff_float[i*3+3] end point_cloud_handle=simGetObjectHandle("Point_cloud")--得到Point_cloud的句柄 simAddPointCloud(0,255,point_cloud_handle,0,pointSize,pts,nil,colors,nil)--添加点云完事儿了 end end --wr simSetJointTargetVelocity(motorLeft,0) simSetJointTargetVelocity(motorRight,0) end

最终效果图：

兔子在Vrep里出现了：