How to use lazer data (hokuyo_urg) in ROS

Typically: "How do I... ", "How can I... " questions
Post Reply
Hrithik Verma
Posts: 22
Joined: 07 Sep 2020, 21:20
Contact:

How to use lazer data (hokuyo_urg) in ROS

Post by Hrithik Verma »

In CoppeliaSim 4.2, I did not find any example or sensor model to hokuyo_urg (lazer) to use it with ROS.
If Possible than please tell me a way to send lazes sensor data to ROS

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

Re: How to use lazer data (hokuyo_urg) in ROS

Post by fferri »

The approach for publishing data to ROS is always the same:

1) create a publisher, with simROS.advertise (normally in sysCall_init)
2) create a message data structure (Lua table), in this case you probably want sensor_msgs/LaserScan:

Code: Select all

local msg={
    header={
        stamp=simROS.getTime(),
        frame_id='...',
    },
    angle_min=0.12
    angle_max=0.62
    angle_increment=0.01
    time_increment=0.002
    scan_time=0.1
    range_min=0.001
    range_max=2.5
    ranges={...},
    intensities={...},
}
check ROS documentation for the meaning of the message data fields.

3) publish the message with simROS.publish

last two steps you normally want to do in sysCall_sensing

Hrithik Verma
Posts: 22
Joined: 07 Sep 2020, 21:20
Contact:

Re: How to use lazer data (hokuyo_urg) in ROS

Post by Hrithik Verma »

Problem is how to extract data from sensors to send it to ROS like how to get range and intensity values. If possible tell me a scene example which I can use.

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: How to use lazer data (hokuyo_urg) in ROS

Post by coppelia »

Take the fast Hokuyo sensor model and replace its script with:

Code: Select all

function sysCall_init()
    maxScanDistance=5
    showLines=true
    generateData=true
    discardMaxDistPts=true

    local self=sim.getObjectHandle(sim.handle:self)
    visionSensors={sim.getObjectHandle("fastHokuyo_sensor1"),sim.getObjectHandle("fastHokuyo_sensor2")}
    sensorRef=sim.getObjectHandle("fastHokuyo_ref")
    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)
end

function sysCall_cleanup() 
    sim.removeDrawingObject(lines)
end 

function sysCall_sensing() 
    local measuredData={} -- relative to sensor ref
    local rangeData={}
    
    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(sensorRef,-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
                        table.insert(rangeData,v[4])
                        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
                    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
end
Then, in each simulation step, you should get the range data in rangeData

Cheers

Post Reply