## fast Hokuyo laser scan data with ROS

Typically: "How do I... ", "How can I... " questions
Thura
Posts: 5
Joined: 13 Jul 2018, 18:27

### fast Hokuyo laser scan data with ROS

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.

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={
{
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

Thanks a lot!

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

### Re: fast Hokuyo laser scan data with ROS

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
Posts: 5
Joined: 13 Jul 2018, 18:27

### Re: fast Hokuyo laser scan data with ROS

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
Posts: 815
Joined: 09 Sep 2013, 19:28

### Re: fast Hokuyo laser scan data with ROS

Thura wrote:
24 Mar 2020, 05:37
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
Posts: 815
Joined: 09 Sep 2013, 19:28

### Re: fast Hokuyo laser scan data with ROS

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
Posts: 5
Joined: 13 Jul 2018, 18:27

### Re: fast Hokuyo laser scan data with ROS

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
Posts: 1
Joined: 05 May 2021, 10:47

### Re: fast Hokuyo laser scan data with ROS

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={
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)
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
Posts: 19
Joined: 07 Sep 2020, 21:20
Contact:

### Re: fast Hokuyo laser scan data with ROS

@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

e-mail I'd - vermahrithik10@gmail.com

Humble request

coppelia
Posts: 8889
Joined: 14 Dec 2012, 00:25

### Re: fast Hokuyo laser scan data with ROS

Hello,

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

self=sim.getObjectHandle(sim.handle_self)
visionSensors={sim.getObjectHandle("fastHokuyo_ROS_sensor1"),sim.getObjectHandle("fastHokuyo_ROS_sensor2")}
local collection=sim.createCollection(0)
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}

end

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

function sysCall_sensing()
local measuredData={}

for i=1,2,1 do
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]
end
end
end
end
end
if #measuredData>0 then
local msg={
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