fast Hokuyo laser scan data with ROS

[Thura]

Hi
I'm trying to send the laser scan data from Hokuyo_URG_04LX on CoppeliaSim to ROS but so far I can't figure out how to make it work correctly.

I could send the transform from laser_frame to world_frame and tested to see it works correctly on rviz.
I created a topic with message, sensor_msgs/LaserScan and packed the sensor informations and data for sending to ROS. But when I view it on Rviz, it appears to be incorrect.

CoppeliaSim image: https://drive.google.com/file/d/1Eu_idq ... sp=sharing

Rviz image: https://drive.google.com/file/d/1ZnWAN9 ... sp=sharing

I think maybe I incorrectly assigned the values to the LaserScan message, although, I have tried many values and searched everywhere without success.

Here is my message,

Code: Select all

function sendLaserData(data)
    local laserDat
    local intensity={10}
    laserDat={
        header=
        {
            stamp=simROS.getTime(),
            frame_id="fastHokuyo"
        },
        angle_min=(-50*(math.pi/180)),
        angle_max=(50*(math.pi/180)),
        angle_increment=(0.1428*(math.pi/180)),
        time_increment=0.5,
        range_min=0.1,
        range_max=5,
        ranges=data,
        intensities=intensity
    }
    simROS.publish(pub, laserDat)
end

Please give me some recommendations
Thanks a lot!

[fferri]

Refer to the documentation of the sensor_msgs/LaserScan message.

CoppeliaSim uses SI units (i.e.: meters, radians, and seconds).

By the way your code doesn't show how you create the data arrays.

[Thura]

I use the array of data from the sensor, "measuredData" , as already implemented in the Hokuyo sensor child script and then pass that data directly into the function for sending to ROS as seen on the previous function code.

This is the portion of code information from CoppeliaSim

Code: Select all

 
    -- measuredData now contains all the points that are closer than the sensor range
    -- For each point there is the x, y and z coordinate (i.e. 3 number for each point)
    -- Coordinates are expressed relative to the sensor frame.
    -- You can access this data from outside via various mechanisms. The best is to first
    -- pack the data, then to send it as a string. For example:
    --

    sendLaserData(measuredData)            -- Assign to ROS message and publish the topic
    

But as seen from the pictures I attached of Coppeliasim and Rviz, The data visualization appears to be incorrect. I don't know what I did wrong here

[fferri]

This is the portion of code information from CoppeliaSim

Code: Select all

 
    -- measuredData now contains all the points that are closer than the sensor range
    -- For each point there is the x, y and z coordinate (i.e. 3 number for each point)

Do you realize sensor_msgs/LaserScan works with points in polar coordinates, and expects points to be spaced evenly between angle_min and angle_max?

http://docs.ros.org/melodic/api/sensor_ ... rScan.html

[fferri]

Note that there is a Hokuyo laser scanner (Hokuyo_URG_04LX_UG01_ROS) which is made specifically for ROS. Athough it is for the old ROS pugin, it might be easier to adjust.

[Thura]

Thank you so much for your advises! I was so confused about the "range" element but things are clearer now.
It didn't occur to me to use the Hokuyo sensor with well implemented code for ROS. Now I'm using it and I've succeeded sending the laser data to Rviz.

Thank a million, friend!

[Gonzalo98]

Hi! With a lot of effort, I finally solved the problem of pusbilshing Hokuyo sensor data to ROS via RosInterface. This solution only works with the non-fast sensor: Hokuyo URG 04LX UG01.ttm . I tried using the fast sensor (Hokuyo URG 04LX UG01 fast.ttm) but there where some errors in distance measurement.

Firstly, I fixed the sendLaserData function.
The number 684 is the amount of points the sensor display inside its range.

Code: Select all

function sendLaserData(data)
    local laserDat
    local intensity={}
    laserDat={
        header={
            stamp=simROS.getTime(),
            frame_id="base_scan"
        },
        -- range = 240 degrees
        angle_min=(-120*(math.pi/180)),
        angle_max=(120*(math.pi/180)),
        angle_increment=(240*math.pi)/(684*180),
        time_increment=(1/50)/684,

        range_min=0.8,
        range_max=5,
        ranges=data,
        intensities=intensity
    }
    -- ROS publish
    simROS.publish(sensorPub, laserDat)
end

ranges is the main field and problem. That should be a list of 684 values where each ot them is the distance from the point sensed to the sensor base, but the hokuyo sensor returns the x,y,z coordinate for each point sensed. In order to fix that, I have implemented this code in at the end of syscall_sensing() function. It takes the x,y,z of each point (stored in points variable) and returns the euclidean distance (in XY plane) of each point stored in dist variable:

Code: Select all

        -- GONZALO GARCIA & JAIME VILLA

    local a = {} -- list {x1, x2, x3, x4...}
    local b = {} -- list {y1, y2, y3, y4...}
    local dist = {} -- list of euclidean distances. It goes on sendLaserData(dist) to ROS
    local temp = 0.0 -- dits of each point
    for i, value in ipairs(points) do
        if (i-1)%3==0 then
            table.insert(a, value) -- stores x_n
            table.insert(b, points[i+1]) -- stores y_n
        end
    end
    
    -- Calc distance from x_n and y_n
    -- value = x_n
    -- b[i] = y_n
    for i, value in ipairs(a) do
        temp = math.sqrt((value*value) + (b[i]*b[i]))
        table.insert(dist, temp)
    end
    
    -- publishing tf (uncomment if needed) and sensor data
    if simROS then
        sendLaserData(dist)
        --simROS.sendTransform(getTransformStamped(modelRef,'base_scan',-1,'base_link'))
    end
    --------------------------------------------------------------------------------

Here there are two pictures as proof of work:

RVIZ final success:
https://ibb.co/10gdQND



Fail with FAST sensor
https://ibb.co/Tr627Yr


I hope this will help anyone trying to solve this issue :)

[Hrithik Verma]

@gonzola98 please can you share your scene where you implemented this hokuyo laser scene

If possible share here so that It will be helpful for others as well and maybe you can request pull this to Coppeliasim GitHub because example like this are not available anywhere

Humble request

[coppelia]

Hello,

please try with this model.
Following is its code (note that the model itself has slightly changed compared to similar models in release V4.2.0 and prior):

Code: Select all

function sysCall_init()
    maxScanDistance=5
    showLines=true
    generateData=true
    rangeData=true -- if false, then X/Y/Z data rel to sensor base
    discardMaxDistPts=true

    self=sim.getObjectHandle(sim.handle_self)
    visionSensors={sim.getObjectHandle("fastHokuyo_ROS_sensor1"),sim.getObjectHandle("fastHokuyo_ROS_sensor2")}
    local collection=sim.createCollection(0)
    sim.addItemToCollection(collection,sim.handle_all,-1,0)
    sim.addItemToCollection(collection,sim.handle_tree,self,1)
    sim.setObjectInt32Param(visionSensors[1],sim.visionintparam_entity_to_render,collection)
    sim.setObjectInt32Param(visionSensors[2],sim.visionintparam_entity_to_render,collection)
    sim.setObjectFloatParam(visionSensors[1],sim.visionfloatparam_far_clipping,maxScanDistance)
    sim.setObjectFloatParam(visionSensors[2],sim.visionfloatparam_far_clipping,maxScanDistance)
    red={1,0,0}
    lines=sim.addDrawingObject(sim.drawing_lines,1,0,-1,10000,nil,nil,nil,red)
    
    pub=simROS.advertise('/hokuyo', 'sensor_msgs/LaserScan')
end

function sysCall_cleanup() 
    simROS.shutdownPublisher(pub)
    sim.removeDrawingObject(lines)
end 

function sysCall_sensing() 
    local measuredData={}
    
    sim.addDrawingObjectItem(lines,nil)
    for i=1,2,1 do
        local r,t,u=sim.readVisionSensor(visionSensors[i])
        if u then
            local sensorM=sim.getObjectMatrix(visionSensors[i],-1)
            local relRefM=sim.getObjectMatrix(self,-1)
            sim.invertMatrix(relRefM)
            relRefM=sim.multiplyMatrices(relRefM,sensorM)
            local p={0,0,0}
            p=sim.multiplyVector(sensorM,p)
            t={p[1],p[2],p[3],0,0,0}
            for j=0,u[2]-1,1 do
                for k=0,u[1]-1,1 do
                    local w=2+4*(j*u[1]+k)
                    local v={u[w+1],u[w+2],u[w+3],u[w+4]}
                    if generateData then
                        if rangeData then
                            table.insert(measuredData,v[4])
                        else
                            if v[4]<maxScanDistance*0.9999 or not discardMaxDistPts then
                                p=sim.multiplyVector(relRefM,v)
                                table.insert(measuredData,p[1])
                                table.insert(measuredData,p[2])
                                table.insert(measuredData,p[3])
                            end
                        end
                    end
                    if showLines then
                        p=sim.multiplyVector(sensorM,v)
                        t[4]=p[1]
                        t[5]=p[2]
                        t[6]=p[3]
                        sim.addDrawingObjectItem(lines,t)
                    end
                end
            end
        end
    end
    if #measuredData>0 then
        local msg={
            header={
                stamp=simROS.getTime(),
                frame_id='a',
            },
            angle_min=-120*math.pi/180,
            angle_max=120*math.pi/180,
            angle_increment=(240/684)*math.pi/180, -- angle: 240 deg, pts: 684
            time_increment=sim.getSimulationTimeStep(),
            scan_time=sim.getSimulationTimeStep(),
            range_min=0.001,
            range_max=maxScanDistance,
            ranges=measuredData,
            intensities={},
        }
        simROS.publish(pub,msg)
    end
end 

Cheers

[Thura]

Thank again so so much! after all these time I still meet with the same problem but now it works like a charm. I can't believe you guys haven't included this ROS implementation model with the installation. There must be a lot of people looking for it.