Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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)
- local ptCloud_topicName='velodyne'
- lidar_pub=simROS.advertise('/'..ptCloud_topicName,'sensor_msgs/PointCloud')
- end
- 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
- 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)
- --]]
- m=sim.getObjectMatrix(visionSensorHandles[1],-1)
- point32 = {}
- 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)
- point32[i+1]={x=d[1], y=d[2], z=d[3]}
- end
- -- Publish the Lidar PointClouds to ROS Topic
- if not pluginNotFound then
- pub_data={}
- pub_data['header']={stamp=sim.getSystemTime(),frame_id="velodyneVPL"}
- pub_data['points']=point32
- simROS.publish(lidar_pub,pub_data)
- simROS.sendTransform(getTransformStamped(visionSensorHandles[1],'velodyneVPL',-1,'world'))
- end
- end
- function sysCall_cleanup()
- simExtVision_destroyVelodyneVPL16(h)
- end
Add Comment
Please, Sign In to add comment