These are major points I set for the RosInterface:
- Set a publisher using sensor_msgs/PointCloud msg
- Publish data with header.frame_id corresponding to the /tf frame_id in simROS.sendTransform, and points = mydata
- What msg type I should use, PointCloud or PointCloud2?
- Which variable I need to take and publish according to the chosen msg type?
- What fields in the msg type are necessary to fill in? And what their value should be? Such as frame_id, width, length, data, what variable in the script should be fed for these fields?
- How does /tf information works? What should I make the /tf topic interact with the point cloud?
Init:
Code: Select all
function sysCall_init()
visionSensorHandles={}
for i=1,4,1 do
visionSensorHandles[i]=sim.getObjectHandle('velodyneVPL_16_sensor'..i)
end
ptCloudHandle=sim.getObjectHandle('velodyneVPL_16_ptCloud')
frequency=5 -- 5 Hz
options=2+8 -- bit0 (1)=do not display points, bit1 (2)=display only current points, bit2 (4)=returned data is polar (otherwise Cartesian), bit3 (8)=displayed points are emissive
pointSize=2
coloring_closeAndFarDistance={1,4}
displayScaling=0.999 -- so that points do not appear to disappear in objects
h=simVision.createVelodyneVPL16(visionSensorHandles,frequency,options,pointSize,coloring_closeAndFarDistance,displayScaling,ptCloudHandle)
-- Check if the required ROS plugin is there:
moduleName=0
moduleVersion=0
index=0
pluginNotFound=true
while moduleName do
moduleName,moduleVersion=sim.getModuleName(index)
if (moduleName=='RosInterface') then
pluginNotFound=false
end
index=index+1
end
-- Ok now launch the ROS client application:
if (not pluginNotFound) then
local ptCloud_topicName='velodyne'
lidar_pub=simROS.advertise('/'..ptCloud_topicName,'sensor_msgs/PointCloud')
end
end
Code: Select all
function sysCall_sensing()
data=simVision.handleVelodyneVPL16(h,sim.getSimulationTimeStep())
-- if we want to display the detected points ourselves:
if ptCloud then
sim.removePointsFromPointCloud(ptCloud,0,nil,0)
else
ptCloud=sim.createPointCloud(0.02,20,0,pointSize)
end
m=sim.getObjectMatrix(visionSensorHandles[1],-1)
for i=0,#data/3-1,1 do
d={data[3*i+1],data[3*i+2],data[3*i+3]}
d=sim.multiplyVector(m,d)
data[3*i+1]=d[1]
data[3*i+2]=d[2]
data[3*i+3]=d[3]
end
sim.insertPointsIntoPointCloud(ptCloud,0,data)
-- Publish the Lidar PointClouds to ROS Topic
if not pluginNotFound then
pub_data={}
pub_data['header']={frame_id="velodyneVPL"}
pub_data['points']=data
simROS.publish(lidar_pub,pub_data)
simROS.sendTransform(getTransformStamped(visionSensorHandles[1],'velodyneVPL',-1,'world'))
end
end
Code: Select all
function getTransformStamped(objHandle,name,relTo,relToName)
t=sim.getSystemTime()
p=sim.getObjectPosition(objHandle,relTo)
o=sim.getObjectQuaternion(objHandle,relTo)
return {
header={
stamp=t,
frame_id=relToName
},
child_frame_id=name,
transform={
translation={x=p[1],y=p[2],z=p[3]},
rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
}
}
end