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

将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点云文件传入Vrep的一种方法_第1张图片
后来我发现pcd文件是一个ascii编码的文件,直接用记事本打开,我们来看一下内容
将PCD点云文件传入Vrep的一种方法_第2张图片
实际上我们需要的数据就是下面这些点的坐标(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得到点云坐标,并增加到点云中。
将PCD点云文件传入Vrep的一种方法_第3张图片
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里出现了:
将PCD点云文件传入Vrep的一种方法_第4张图片

你可能感兴趣的:(将PCD点云文件传入Vrep的一种方法)