Problem with publish sensor_msgs/PointCloud2 format kinect data

Typically: "How do I... ", "How can I... " questions
Post Reply
lyz
Posts: 22
Joined: 07 Mar 2019, 07:34

Problem with publish sensor_msgs/PointCloud2 format kinect data

Post by lyz » 13 May 2019, 14:02

Hi Coppelia,
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)
Then, I have several problems:
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.
Hope you can help me.

Thank you in advance.

Sincerely,

lyz

lyz
Posts: 22
Joined: 07 Mar 2019, 07:34

Re: Problem with publish sensor_msgs/PointCloud2 format kinect data

Post by lyz » 14 May 2019, 11:23

I have solved this problem!

magnetica
Posts: 5
Joined: 06 Apr 2019, 18:52

Re: Problem with publish sensor_msgs/PointCloud2 format kinect data

Post by magnetica » 31 May 2019, 06:13

Hi lyz

I am facing the similar issue of publishing the point cloud data from velodyne sensor and posted the query for the same. Could you please share your solution?

Thanks

lyz
Posts: 22
Joined: 07 Mar 2019, 07:34

Re: Problem with publish sensor_msgs/PointCloud2 format kinect data

Post by lyz » 04 Jun 2019, 07:15

Hello,
I modified the filter dialog of visual sensor, add Extract coordinates from work image(32, 32) , and the resolution x/y is 32.
The kinect_pointcloud settings is :
1. Original depth image to work image;
2. Extract coordinates from work image;
3. Intensity scale work image
4. work image to output image.
you can try first. If not, please tell me.

Post Reply