Interface between Velodyne VPL16 and ROS

Typically: "How do I... ", "How can I... " questions
Post Reply
Rick Shen
Posts: 4
Joined: 15 Oct 2018, 01:30

Interface between Velodyne VPL16 and ROS

Post by Rick Shen »

Hi, I am recently using Velodyne VPL 16 for a car simulation. I need to transmit the detected point cloud to ROS via the RosInterface. I am modifying the template code coming along with the Velodyne model. I try to use Rviz to visualize them but there is no point showing in the window even though Rviz shows the messages are successfully received.

These are major points I set for the RosInterface:
  1. Set a publisher using sensor_msgs/PointCloud msg
  2. Publish data with header.frame_id corresponding to the /tf frame_id in simROS.sendTransform, and points = mydata
I am quite new for the Velodyne model, point cloud msg and Rviz, can you please help me with this, especially the following questions?
  1. What msg type I should use, PointCloud or PointCloud2?
  2. Which variable I need to take and publish according to the chosen msg type?
  3. 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?
  4. How does /tf information works? What should I make the /tf topic interact with the point cloud?
My code is here:

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
Sensing:

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
getTransformStamped Function

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
I searched the previous discussions in the forum but those API functions are totally different from the current version. I will be grateful if someone can help me through this!!

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

Re: Interface between Velodyne VPL16 and ROS

Post by fferri »

Do you see the data arriving at the /velodyne topic?

To test, run the following in a terminal:

Code: Select all

rostopic echo /velodyne
If yes, then it might be a problem of choosing the correct frame in RViz (try selecting the velodyneVPL frame).

If in RViz you select the /map frame, but no tf frame linking /map to /velodyneVPL is broadcasted, RViz won't show you anything (it will show an error in the PointCloud display however).

Rick Shen
Posts: 4
Joined: 15 Oct 2018, 01:30

Re: Interface between Velodyne VPL16 and ROS

Post by Rick Shen »

fferri wrote: 16 Oct 2018, 11:04

Code: Select all

rostopic echo /velodyne
If yes, then it might be a problem of choosing the correct frame in RViz (try selecting the velodyneVPL frame).

If in RViz you select the /map frame, but no tf frame linking /map to /velodyneVPL is broadcasted, RViz won't show you anything (it will show an error in the PointCloud display however).
Thanks for the reply! I have checked that /velodyne topic has data arriving and RViz can find the /velodyneVPL frame. Therefore, I am now paying more attention to the msg type.

Should I use PointCloud2 or PointCloud?

If it should be PointCloud2, which of these fields are mandatory and which are neglectable, and how to fill them from velodyne template code:
  • std_msgs/Header header
  • uint32 height (Where should I find heigh and width in velodyne template)
  • uint32 width
  • sensor_msgs/PointField[] fields
  • bool is_bigendian
  • uint32 point_step
  • uint32 row_step
  • uint8[] data (why it is uint8 here. If I assign results from velodyne to it, I only see a lot 0, 1 in rostopic echo)
  • bool is_dense
If it should be PointCloud, how should I do with these:
  • std_msgs/Header header
  • geometry_msgs/Point32[] points
  • sensor_msgs/ChannelFloat32[] channels
Especially, the "points" field requires geometry_msgs/Point32[]. When I want to feed it with results from velodyne, error shows up and saying it is not the right type. How should I convert it?

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

Re: Interface between Velodyne VPL16 and ROS

Post by fferri »

Whether you should use sensor_msgs/PointCloud or sensor_msgs/PointCloud2 depends on which message types the receiving part can support.

If it is RViz, it is capable of displaying both.

The main difference is that PointCloud contains point data stored as arrays of points and float values, and it is simpler to fill. PointCloud2's data is stored in a binary blob, and more efficient in terms of space, but you have to store additional information such as point size in bytes (point_step field), stride (row step field), and so on; all of those fields are mandatory, and the documentation is contained in the links you posted, i.e. http://docs.ros.org/api/sensor_msgs/htm ... loud2.html.

See also https://answers.ros.org/question/207071 ... in-python/

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

Re: Interface between Velodyne VPL16 and ROS

Post by fferri »

Rick Shen wrote: 16 Oct 2018, 17:27Especially, the "points" field requires geometry_msgs/Point32[]. When I want to feed it with results from velodyne, error shows up and saying it is not the right type. How should I convert it?
since the points field is an array of Point32 message, it should be something like this (in Lua):

Code: Select all

{points={{x=0.0,y=0.0,z=0.1},{x=0.1,y=0.0,z=0.12},...}}

Rick Shen
Posts: 4
Joined: 15 Oct 2018, 01:30

Re: Interface between Velodyne VPL16 and ROS

Post by Rick Shen »

This ROS answer is so helpful! Now I can properly set the data structure and I can see them in Rviz. I use PointCloud msg type since I think PointCloud2 may need some special conversion.

A new problem, after the simulation starts, there will always be a message box bumping out and saying my velodyne code has "Abort execution". The detection is running very slow that it takes a lot of time to finish the detection in the four direction. The effect is very bad that I can see the point cloud in each direction stays for about 3 sec and then the next one comes. This happens also in Rviz.

Below are my complete code. Is there any method to speed up or it is just because my laptop is bad? (It is indeed an old laptop and I am even using Virtual Machine for ROS and Vrep)

Code: Select all

function sysCall_init()

    isOpen=sim.getScriptSimulationParameter(sim.handle_self,'isOpen')

    if (isOpen==1) then

        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

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()

    if (isOpen==1) then

        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)
    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)

        data[3*i+1]=d[1]

        data[3*i+2]=d[2]

        data[3*i+3]=d[3]
        point32[i+1]={x=d[1], y=d[2], z=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

end

function sysCall_cleanup()

    if (isOpen==1) then

        simExtVision_destroyVelodyneVPL16(h)

    end

end
BTW, I hope the code can be helpful for anyone else who wants this interface.

Post Reply