Lidar velodyne Vrep

gistfile1.txt

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