How to show pointcloud msg from ROS

Typically: "How do I... ", "How can I... " questions
Post Reply
liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

How to show pointcloud msg from ROS

Post by liyinghui »

I want to show the point cloud msg in vrep which is from ROS, what should i do? Actually, i m new to that and I could get the pointcloud image in rviz. But I want to show it in vrep so that i could simulate my robot with the pointcloud data.

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

Re: How to show pointcloud msg from ROS

Post by coppelia »

Hello,

from the top of my head, something like:

Code: Select all

-- Subscribe:
sub=simROS.subscribe('/ptcloud', 'sensor_msgs/PointCloud', 'ptcloudMessage_callback')

-- Subscriber callback:
function ptcloudMessage_callback(msg)
    sim.removePointsFromPointCloud(ptCloudHandle,0,nil,0)
    local pts={}
    for i=1,msg.points,1 do
        pts[#pts+1]=msg.points[i].x
        pts[#pts+1]=msg.points[i].y
        pts[#pts+1]=msg.points[i].z
    end
    sim.insertPointsIntoPointCloud(ptCloudHandle,0,pts)
end
Keep in mind that the point cloud object must already be created, and that with above code things can get quite slow with larger point clouds. Another message could be more convenient, i.e. one that has all point data inside of a single float array.

Cheers

liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

Re: How to show pointcloud msg from ROS

Post by liyinghui »

Glad to receive your reply! And i have tried it:

Code: Select all

function sysCall_init()
    -- pc=sim.createPointCloud(0.01,1000,0,2)
    -- Get some handles:
    pc=sim.getObjectHandle('Point_cloud')

    -- Enable an image publisher and subscriber:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        sub=simROS.subscribe('/points2', 'sensor_msgs/PointCloud2', 'PcMessage_callback')
        -- simROS.subscriberTreatUInt8ArrayAsString(sub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function PcMessage_callback(msg)
    -- Apply the received image to the passive vision sensor that acts as an image container
    sim.removePointsFromPointCloud(pc,0,nil,0)
    local pts={}
    for i=1,msg.points,1 do
        pts[#pts+1]=msg.points[i].x
        pts[#pts+1]=msg.points[i].y
        pts[#pts+1]=msg.points[i].z
    end
    sim.insertPointsIntoPointCloud(pc,0,pts)
end


function sysCall_cleanup()
    if simROS then   
        -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
        simROS.shutdownSubscriber(sub)
    end
end
I have set one pointcloud, and implement the code above. But the scene stuck with the errors:
[sandboxScript:info] Simulation started.
[Point_cloud@childScript:info] ROS interface was found.
[Point_cloud@childScript:error] 30: 'for' limit must be a number
stack traceback:
[string "Point_cloud@childScript"]:30: in function <[string "Point_cloud@childScript"]:26>
[CoppeliaSim:error] External call to simCallScriptFunction failed (PcMessage_callback): Error in script function.
[Point_cloud@childScript:error] 30: 'for' limit must be a number
stack traceback:
[string "Point_cloud@childScript"]:30: in function <[string "Point_cloud@childScript"]:26>
[CoppeliaSim:error] External call to simCallScriptFunction failed (PcMessage_callback): Error in script function.
[Point_cloud@childScript:error] 30: 'for' limit must be a number
stack traceback:
[string "Point_cloud@childScript"]:30: in function <[string "Point_cloud@childScript"]:26>
[CoppeliaSim:error] External call to simCallScriptFunction failed (PcMessage_callback): Error in script function.
[sandboxScript:info] simulation stopping...
[Point_cloud@childScript:error] 30: 'for' limit must be a number
stack traceback:
[string "Point_cloud@childScript"]:30: in function <[string "Point_cloud@childScript"]:26>
[CoppeliaSim:error] External call to simCallScriptFunction failed (PcMessage_callback): Error in script function.
[sandboxScript:info] Simulation stopped.
And my camera is Azure kinect dk, and it will offer one pointcloud with 360000 points. I doubt that error was caused by the millions points?

liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

Re: How to show pointcloud msg from ROS

Post by liyinghui »

Image. looking at rqt_graph I have get this topic in vrep. I think the problem is caused by the difference between poincloud2 and pointcloud. what should i do?

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

Re: How to show pointcloud msg from ROS

Post by coppelia »

Yes, it is better to use sensor_msgs/PointCloud2. Then, make sure to enable following for the subscriber:

Code: Select all

-- ROS:
simROS.subscriberTreatUInt8ArrayAsString(sub)
-- ROS2:
simROS2.subscriptionTreatUInt8ArrayAsString(sub)
Then convert the returned array to a table with sim.unpackFloatTable

Cheers

liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

Re: How to show pointcloud msg from ROS

Post by liyinghui »

I followed your instruction, but i still get the errors like that:

Code: Select all

 31: attempt to index field 'points' (a nil value)
    stack traceback:
        [string "Point_cloud@childScript"]:31: in function <[string "Point_cloud@childScript"]:26>
 
It is seemed like the unpack processing is wrong.
This is the code I have now:

Code: Select all


function sysCall_init()
    -- pc=sim.createPointCloud(0.01,1000,0,2)
    -- Get some handles:
    pc=sim.getObjectHandle('Point_cloud')

    -- Enable an image publisher and subscriber:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        sub=simROS.subscribe('/points2', 'sensor_msgs/PointCloud2', 'PcMessage_callback')
        simROS.subscriberTreatUInt8ArrayAsString(sub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function PcMessage_callback(msg)
    -- Apply the received image to the passive vision sensor that acts as an image container
    sim.removePointsFromPointCloud(pc,0,nil,0)
    local pts={}
    for i=1,msg.points,1 do
        pts[#pts+1]=msg.points[i].x
        pts[#pts+1]=msg.points[i].y
        pts[#pts+1]=msg.points[i].z
    end
    pts=sim.unpackFloatTable(pts)
    sim.insertPointsIntoPointCloud(pc,0,pts)
end


function sysCall_cleanup()
    if simROS then   
        -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
        simROS.shutdownSubscriber(sub)
    end
end
I'd appreciate if you can give more help!!!

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

Re: How to show pointcloud msg from ROS

Post by coppelia »

You are mixing up PointCloud and PointCloud2 messages!

Cheers

liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

Re: How to show pointcloud msg from ROS

Post by liyinghui »

https://youtu.be/KWXOJPNQjB4
Thanks Gods,I finally get the pointcloud in vrep, but unluckily there is a few errors still, It seems like that the pointcloud is duplicated, what wrongs have i made?

Code: Select all

function sysCall_init()
    -- pc=sim.createPointCloud(0.01,1000,0,2)
    -- Get some handles:
    pc=sim.getObjectHandle('Point_cloud')

    -- Enable an image publisher and subscriber:
    if simROS then
        sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
        sub=simROS.subscribe('/points2', 'sensor_msgs/PointCloud2', 'PcMessage_callback')
        simROS.subscriberTreatUInt8ArrayAsString(sub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
    else
        sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
    end
end

function PcMessage_callback(msg)
    -- Apply the received image to the passive vision sensor that acts as an image container
    sim.removePointsFromPointCloud(pc,0,nil,0)
    data=sim.unpackFloatTable(msg.data)
    --local pts={}
    --for i=1,#data/3,1 do
       -- pts[#pts+1]={data[i],data[i+1],data[i+2]}
    --end
    sim.insertPointsIntoPointCloud(pc,0,data,{0,255,0})
end


function sysCall_cleanup()
    if simROS then   
        -- Shut down publisher and subscriber. Not really needed from a simulation script (automatic shutdown)
        simROS.shutdownSubscriber(sub)
    end
end

liyinghui
Posts: 11
Joined: 26 Apr 2022, 03:37

Re: How to show pointcloud msg from ROS

Post by liyinghui »

I m struggle with how to get the real data from pointcloud2 msg. I could not get it from the msg.data. how should i code?

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: How to show pointcloud msg from ROS

Post by fferri »

sensor_msgs/PointCloud2 is a binary format.

The data field is a sequence of bytes (in CoppeliaSim that may come as uint8 array or a string, depending on the subscriber settings). Information to decode it is contained in the other fields of the message (is_bigendian, fields, point_step, row_step, ...).

Refer to the message documentation for more info.

In CoppeliaSim, Packing/unpacking functions may be useful.

Post Reply