## Importing and visualizing pointcloud

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

### Importing and visualizing pointcloud

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.

Sincerely,

Tiber

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

### Re: Importing and visualizing pointcloud

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

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

coppelia
Posts: 7928
Joined: 14 Dec 2012, 00:25

### Re: Importing and visualizing pointcloud

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

Cheers

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

### Re: Importing and visualizing pointcloud

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)
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