Page 1 of 1

Importing and visualizing pointcloud

Posted: 07 Dec 2018, 16:44
by Tiber
Hi,I am a newbie using V-REP and I’m trying to subscribe a ros topic of type sensor_msg/Pointcloud2 and show its content into a pointcloud object.But I can’t get the function sim.insertPointsIntoPointCloud working.The errors I get are “One of the function's argument type is not correct” and “[string -unknown location]:?: ros_callback__sensor_msgs__PointCloud2: simCallScriptFunctionEx: error (pointcloudMessage_callback)”.
The code I am using:

Code: Select all

function sysCall_init()
    size=1
    col={100,100,100}
    pc=sim.createPointCloud(0.02,20,0,size)
    sub=simROS.subscribe('/camera/depth_registered/points', 'sensor_msgs/PointCloud2', 'pointcloudMessage_callback')
    simROS.subscriberTreatUInt8ArrayAsString(sub)
end
function pointcloudMessage_callback(pts)
    local points={}
    local num=pts.width*pts.height
    for i=0,num-1 do
        table.insert(points,pts.data[3*i])
        table.insert(points,pts.data[3*i+1])
        table.insert(points,pts.data[3*i+2])
    end
    sim.insertPointsIntoPointCloud(pc,0,ponits,col,nil)
end
Hope someone can help me.

Thank you in advance.

Sincerely,

Tiber

Re: Importing and visualizing pointcloud

Posted: 07 Dec 2018, 17:43
by gendibal784
It seems to me that you don't define the variable nil on your code

Code: Select all

sim.insertPointsIntoPointCloud(pc,0,ponits,col,nil)
Perhaps it's the reason because of the error say [string -unknown location]

Re: Importing and visualizing pointcloud

Posted: 08 Dec 2018, 02:13
by Tiber
I think nil is not a variable but a type represents a case of "non-value".Am I wrong?

Re: Importing and visualizing pointcloud

Posted: 10 Dec 2018, 15:04
by coppelia
you wrote ponits, but it should be points...

Cheers

Re: Importing and visualizing pointcloud

Posted: 13 Dec 2018, 06:39
by Tiber
Thanks a lot! But I realized this was not where the main problem lies. The main reason is that I got the data type wrong.The point data of type sensor_msgs/PointCloud2 is stored as a Uint8 array and every four elements represent a component of the coordinates of a point. And the function simROS.subscriberTreatUInt8ArrayAsString treats uint8 arrays as string, so the data obtained by the subscriber should be converted to the Float type to obtain the coordinate information and to the Uint8 type to obtain the RGB information separately.
The code I am using now:

Code: Select all

function sysCall_init()
    size=1
    pc=sim.createPointCloud(0.02,20,0,size)
    pointcloud=sim.getObjectHandle('pointcloud')
    depthCam=sim.getObjectHandle('kinect_depth')
    sim.setObjectMatrix(pc,-1,sim.getObjectMatrix(depthCam,-1))
    sub=simROS.subscribe('/camera/depth_registered/points', 'sensor_msgs/PointCloud2', 'pointcloudMessage_callback')
    simROS.subscriberTreatUInt8ArrayAsString(sub)
end
function pointcloudMessage_callback(pts)
    sim.removePointsFromPointCloud(pc,0,nil,0)
    local start=sim.getSystemTimeInMs(-2)
    local points={}
    local col={}
    local num=pts.width*pts.height
    tables=sim.unpackFloatTable(pts.data,0,0,0)
    tables1=sim.unpackUInt8Table(pts.data,0,0,0)
    local unpack=sim.getSystemTimeInMs(-2)
    for  i=0,num-1 do
        if(tables[8*i+1]==tables[8*i+1]) then
           table.insert(col,tables1[32*i+19])
           table.insert(col,tables1[32*i+18])
           table.insert(col,tables1[32*i+17])
           table.insert(points,-tables[8*i+1])
           table.insert(points,-tables[8*i+2])
           table.insert(points,tables[8*i+3])
         end
    end
    local extract=sim.getSystemTimeInMs(-2)
    sim.insertPointsIntoPointCloud(pc,3,points,col,nil)
    local insert=sim.getSystemTimeInMs(-2)
    time1=unpack-start
    time2=extract-unpack
    time3=insert-extract
    dlgHandle=nil
    local txt='unpack_time='..time1.."ms    extract_time="..time2.."ms    insert_time="..time3.."ms"
    dlgHandle=sim.displayDialog('info',txt,sim.dlgstyle_message,false)
    sim.switchThread()
end

Hope it can help others.
And I didn't take into account its efficiency. If you have a better solution, welcome to share with me.
Thank you!

Re: Importing and visualizing pointcloud

Posted: 14 Dec 2018, 08:31
by coppelia
Maybe the following API function can help: sim.transformBuffer

Cheers