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
How to use lazer data (hokuyo_urg) in ROS
-
- Posts: 22
- Joined: 07 Sep 2020, 21:20
- Contact:
Re: How to use lazer data (hokuyo_urg) in ROS
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:
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
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={...},
}
3) publish the message with simROS.publish
last two steps you normally want to do in sysCall_sensing
-
- Posts: 22
- Joined: 07 Sep 2020, 21:20
- Contact:
Re: How to use lazer data (hokuyo_urg) in ROS
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.
Re: How to use lazer data (hokuyo_urg) in ROS
Take the fast Hokuyo sensor model and replace its script with:
Then, in each simulation step, you should get the range data in rangeData
Cheers
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
Cheers