Importing and visualizing pointcloud

Typically: "How do I... ", "How can I... " questions
Post Reply
Tiber
Posts: 3
Joined: 07 Dec 2018, 09:52

Importing and visualizing pointcloud

Post by Tiber » 07 Dec 2018, 16:44

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

gendibal784
Posts: 12
Joined: 15 Mar 2018, 23:26

Re: Importing and visualizing pointcloud

Post by gendibal784 » 07 Dec 2018, 17:43

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]

Tiber
Posts: 3
Joined: 07 Dec 2018, 09:52

Re: Importing and visualizing pointcloud

Post by Tiber » 08 Dec 2018, 02:13

I think nil is not a variable but a type represents a case of "non-value".Am I wrong?

coppelia
Site Admin
Posts: 6867
Joined: 14 Dec 2012, 00:25

Re: Importing and visualizing pointcloud

Post by coppelia » 10 Dec 2018, 15:04

you wrote ponits, but it should be points...

Cheers

Tiber
Posts: 3
Joined: 07 Dec 2018, 09:52

Re: Importing and visualizing pointcloud

Post by Tiber » 13 Dec 2018, 06:39

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!

coppelia
Site Admin
Posts: 6867
Joined: 14 Dec 2012, 00:25

Re: Importing and visualizing pointcloud

Post by coppelia » 14 Dec 2018, 08:31

Maybe the following API function can help: sim.transformBuffer

Cheers

Post Reply