How to show pointcloud msg from ROS
How to show pointcloud msg from ROS
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.
Re: How to show pointcloud msg from ROS
Hello,
from the top of my head, something like:
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
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
Cheers
Re: How to show pointcloud msg from ROS
Glad to receive your reply! And i have tried it:
I have set one pointcloud, and implement the code above. But the scene stuck with the errors:
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
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?[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.
Re: How to show pointcloud msg from ROS
. 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?
Re: How to show pointcloud msg from ROS
Yes, it is better to use sensor_msgs/PointCloud2. Then, make sure to enable following for the subscriber:
Then convert the returned array to a table with sim.unpackFloatTable
Cheers
Code: Select all
-- ROS:
simROS.subscriberTreatUInt8ArrayAsString(sub)
-- ROS2:
simROS2.subscriptionTreatUInt8ArrayAsString(sub)
Cheers
Re: How to show pointcloud msg from ROS
I followed your instruction, but i still get the errors like that:
It is seemed like the unpack processing is wrong.
This is the code I have now:
I'd appreciate if you can give more help!!!
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>
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
Re: How to show pointcloud msg from ROS
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?
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
Re: How to show pointcloud msg from ROS
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?
Re: How to show pointcloud msg from ROS
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.