将PCD点云文件传入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里出现了：