i'm currently trying to extract a pointcloud from a kinect. At the beginning, I published the depth_image and the rgb_image, but I don't know how to publish the sensor_msgs/pointcloud2 format data, So I tried to follow this post(viewtopic.php?t=5662), This is the code I'm using:
Code: Select all
msg_pointcloud={}
msg_pointcloud['header']={seq=0,stamp=simROS.getTime(), frame_id="camera_front_up_link"}
msg_pointcloud['height']=res[2]
msg_pointcloud['width']=res[1]
msg_pointcloud['is_dense']=true
msg_pointcloud['fields'] = { { name='x', offset=0, datatype= 7,count=1 } ,
{ name='y', offset=4, datatype= 7,count=1 } ,
{ name='z', offset=8, datatype= 7,count=1 } }
msg_pointcloud['is_bigendian']=false
msg_pointcloud['point_step']=12
msg_pointcloud['row_step']=res[1]*12
r,t1,u1=sim.readVisionSensor(activeVisionSensor)
print(u1) -- it is nil???
if u1 then
for j=0,u1[2]-1,1 do
for i=0,u1[1]-1,1 do
local w=2+4*(j*u1[1]+i)
if u1[w+4]<(maxDist*0.9999) then
local p={u1[w+1],u1[w+2],u1[w+3]}
p=sim.multiplyVector(m1,p)
table.insert(measuredData,p[1])
table.insert(measuredData,p[2])
table.insert(measuredData,p[3])
end
end
end
end
msg_pointcloud['data']=sim.packFloatTable(measuredData)
simROS.publish(point_pub, msg_pointcloud)
First, I don't know how to calculate the value(msg_pointcloud['data']);
Second, the u1 is a nil, and it is a mistake;
Finally, this line of code('simROS.publish(point_pub, msg_pointcloud)') reports an error:
Code: Select all
Lua runtime error: [string "CHILD SCRIPT kinect_front_up"]:125: One of the function's argument type is not correct. (sim.packFloatTable)
stack traceback:
[C]: in function 'packFloatTable'
[string "CHILD SCRIPT kinect_front_up"]:125: in function <[string "CHILD SCRIPT kinect_front_up"]:56>
Simulation stopped.
Thank you in advance.
Sincerely,
lyz