Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Typically: "How do I... ", "How can I... " questions
Post Reply
sradmard
Posts: 11
Joined: 06 Oct 2016, 00:39

Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by sradmard » 16 Nov 2016, 00:06

Hi,

I am trying to implement a rosInterface version of Hokuyo_URG_04LX_UG01_ROS sensor so that it uses rosInterface instead of rosPlugin. I would really appreciate if you can advice me on how to do it, or where I can get the attributes of the sensor so that I can define all of the fields of sensor_msgs/LaserScan message type.

Thank you so much in advance.

sradmard
Posts: 11
Joined: 06 Oct 2016, 00:39

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by sradmard » 06 Dec 2016, 21:53

Hi,

I was wondering whether anybody has found the answer or knows where to find the attributes of the sensor.

Cheers

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

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by coppelia » 07 Dec 2016, 13:01

Hello,

what field elements do you need for your message? The whole operation of the Hokuyo is defined in its attached child script. Beware that the Hokuyo URG 04LX UG01_ROS.ttm model is based on a proximity sensor attached to a joint. This type of sensor is most of the time slower than scanners that are based on vision sensors (e.g. Hokuyo URG 04LX UG01_FAST.ttm

Cheers

karmeluk
Posts: 2
Joined: 09 Apr 2018, 14:22

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by karmeluk » 09 Apr 2018, 19:04

Hello,

I have the same trouble.
The suggested by you sensor model "Hokuyo URG 04LX UG01_ROS.ttm" is based on RosPlugin instead of RosInterFace. So I didn't find any example how to publish correct LaserScan message to ROS topic.
My VREP version is 3.5.0 and it seems like precompiled RosPlugin '.so' lib and its source files to compile do not included there.
Currently I am facing with putting a full LaserScan message into publisher.
In included example (it is for RosPlugin (!) as was already mentioned) sensing is implemented in 'sysCall_sensing()' function and sent as a signal:

Code: Select all

    -- Now send the data:
    if #points>0 then
        table.insert(dists,-scanRange*0.5)  --append angle_min to [ranges]
        table.insert(dists,scanRange*0.5-stepSize) --append angle_max to [ranges]
        table.insert(dists,stepSize) --append stepsize to [ranges]
        -- The publisher defined further up uses the data stored in signal 'rangeDat'..objName
        sim.setStringSignal('rangeDat'..objName,sim.packFloatTable(dists))
    end
Next it is handled by rosPlugin publisher:

Code: Select all

topicName=simExtROS_enablePublisher('front_scan',1,simros_strmcmd_get_laser_scanner_data ,modelHandle,-1,'rangeDat'..objName)
where main conversion was prepared by 'simros_strmcmd_get_laser_scanner_data' function which is currently unavailable. It did not find any API or implementation description for this.


I am trying to do it with rosInterface in accordance to included in 3.5.0 release examples as recommended by you and currently the only possible way :

keep sensing as it is with sending as '''rangeDat'..objName" signal:

Code: Select all

function sysCall_sensing() 
    showLaserPoints=sim.getScriptSimulationParameter(sim.handle_self,'showLaserPoints')
    showLaserSegments=sim.getScriptSimulationParameter(sim.handle_self,'showLaserSegments')
    dists={}
    angle=-scanRange*0.5
    sim.setJointPosition(jointHandle,angle)
    jointPos=angle
    
    laserOrigin=sim.getObjectPosition(jointHandle,-1)
    modelInverseMatrix=simGetInvertedMatrix(sim.getObjectMatrix(modelRef,-1))
    
    for ind=0,pts-1,1 do
    
        r,dist,pt=sim.handleProximitySensor(laserHandle) -- pt is relative to the laser ray! (rotating!)
        m=sim.getObjectMatrix(laserHandle,-1)
        if r>0 then
            dists[ind]=dist
            -- We put the RELATIVE coordinate of that point into the table that we will return:
            ptAbsolute=sim.multiplyVector(m,pt)
            ptRelative=sim.multiplyVector(modelInverseMatrix,ptAbsolute)
            points[3*ind+1]=ptRelative[1]
            points[3*ind+2]=ptRelative[2]
            points[3*ind+3]=ptRelative[3]
            segments[7*ind+7]=1 -- indicates a valid point
        else
            dists[ind]=0
            -- If we didn't detect anything, we specify (0,0,0) for the coordinates:
            ptAbsolute=sim.multiplyVector(m,{0,0,6})
            points[3*ind+1]=0
            points[3*ind+2]=0
            points[3*ind+3]=0
            segments[7*ind+7]=0 -- indicates an invalid point
        end
        segments[7*ind+1]=laserOrigin[1]
        segments[7*ind+2]=laserOrigin[2]
        segments[7*ind+3]=laserOrigin[3]
        segments[7*ind+4]=ptAbsolute[1]
        segments[7*ind+5]=ptAbsolute[2]
        segments[7*ind+6]=ptAbsolute[3]
    
        ind=ind+1
        angle=angle+stepSize
        jointPos=jointPos+stepSize
        sim.setJointPosition(jointHandle,jointPos)
    end
    
    
    
    sim.addDrawingObjectItem(lines100,nil)
    sim.addDrawingObjectItem(points100,nil)
    
    if (showLaserPoints or showLaserSegments) then
        t={0,0,0,0,0,0}
        for i=0,pts-1,1 do
            t[1]=segments[7*i+4]
            t[2]=segments[7*i+5]
            t[3]=segments[7*i+6]
            t[4]=segments[7*i+1]
            t[5]=segments[7*i+2]
            t[6]=segments[7*i+3]
            if showLaserSegments then
                sim.addDrawingObjectItem(lines100,t)
            end
            if (showLaserPoints and segments[7*i+7]~=0)then
                sim.addDrawingObjectItem(points100,t)
            end
        end
    end
    
    
    -- Now send the data:
    if #points>0 then
        table.insert(dists,-scanRange*0.5)  --append angle_min to [ranges]
        table.insert(dists,scanRange*0.5-stepSize) --append angle_max to [ranges]
        table.insert(dists,stepSize) --append stepsize to [ranges]
        -- The publisher defined further up uses the data stored in signal 'rangeDat'..objName
        sim.setStringSignal('rangeDat'..objName,sim.packFloatTable(dists))
    end
end 

initiate publisher inside of sysCall_init()

Code: Select all

hokuyoPub = simROS.advertise('/scan','sensor_msgs/LaserScan')
then in sysCall_actuation() publish messages:

I have tried three different cases:

1)

Code: Select all

function sysCall_actuation()
    local laser_scan = sim.getStringSignal('rangeDat'..objName)
        if laser_scan then
            local laser_scan=sim.unpackFloatTable(laser_scan)
            simROS.publish(hokuyoPub, laser_scan) -- and with {laser_scan}
        end
end
error is:
Lua runtime error: [string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:62: read__sensor_msgs__LaserScan: expected a table (simROS.publish @ 'RosInterface' plugin)
stack traceback:
[C]: in function 'publish'
[string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:62: in function <[string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:58>
2)

Code: Select all

function sysCall_actuation()
    local laser_scan = sim.getStringSignal('rangeDat'..objName)
        if laser_scan then
            simROS.publish(hokuyoPub, {msg=sim.unpackFloatTable(laser_scan)})
        end
end
and getting following error:
ua runtime error: [string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:61: read__sensor_msgs__LaserScan: unexpected key: msg (simROS.publish @ 'RosInterface' plugin)
stack traceback:
[C]: in function 'publish'
[string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:61: in function <[string "CHILD SCRIPT Hokuyo_URG_04LX_UG01_ROS"]:58>
Simulation stopped.
3) use 'ranges' field

Code: Select all

function sysCall_actuation()
    local laser_scan = sim.getStringSignal('rangeDat'..objName)
        if laser_scan then
            simROS.publish(hokuyoPub, {ranges=sim.unpackFloatTable(laser_scan)})
        end
end
causes no error but incorrect messages (sniffed by 'rostopic echo /scan'):
---
header:
seq: 147
stamp:
secs: 0
nsecs: 0
frame_id: ''
angle_min: 0.0
angle_max: 0.0
angle_increment: 0.0
time_increment: 0.0
scan_time: 0.0
range_min: 0.0
range_max: 0.0
ranges: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -2.356194496154785, 2.3500585556030273, 0.006135923322290182]
intensities: []

Another thing is how to publish TF together with LaserScan?

Thanks in advance!

/Andy

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

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by fferri » 09 Apr 2018, 22:55

in what sense the message is "incorrect"?

karmeluk
Posts: 2
Joined: 09 Apr 2018, 14:22

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by karmeluk » 10 Apr 2018, 08:40

Because it is empty, even angle parameters. The only 3 incorrect (because as far as I understand LaserScan message and physics it cannot be negative) values in the end of 'ranges' array are present.
I have scene with many different obstacles and expecting LaserScan message fulfilled: : http://docs.ros.org/api/sensor_msgs/htm ... rScan.html

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

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by fferri » 28 Apr 2018, 16:35

You are not setting any message fields.

You have to create the message in Lua according to the message specification, e.g.:

Code: Select all

msg={
    header={
        timestamp=...,
        frame_id=...,
    },
    angle_min=...,
    angle_max=...,
    angle_increment=...,
    time_increment=...,
    scan_time=...,
    range_min=...,
    range_max=...,
    ranges=[....],
    intensities=[...]
}
and then call:

Code: Select all

simROS.publish(hokuyoPub, msg)

matheus.pinto
Posts: 1
Joined: 30 Apr 2018, 13:35

Re: Hokuyo_URG_04LX_UG01_ROS with rosInterface.

Post by matheus.pinto » 04 May 2018, 00:02

Does anyone have a code sample?

Post Reply