## VREP, laser scan, Rviz problem

Typically: "How do I... ", "How can I... " questions
makbut
Posts: 10
Joined: 29 Dec 2016, 11:39

### VREP, laser scan, Rviz problem

Hello all, I am new to V-REP and ROS and it seems I face some problems.

I have the following script for a Hokuyo laser scanner:

Code: Select all

-- This is a ROS enabled Hokuyo_04LX_UG01 model (although it can be used as a generic
-- ROS enabled laser scanner), based on the existing Hokuyo model. It performs instantaneous
-- scans and publishes ROS Laserscan msgs, along with the sensor's tf.
function getTransformStamped(objHandle,name,relTo,relToName)
-- This function retrieves the stamped transform for a specific object
t=simGetSystemTime()
p=simGetObjectPosition(objHandle,relTo)
o=simGetObjectQuaternion(objHandle,relTo)
return {
stamp=t,
frame_id=relToName
},
child_frame_id=name,
transform={
translation={x=p[1],y=p[2],z=p[3]},
rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
}
}
end
if (sim_call_type==sim_childscriptcall_initialization) then
laserHandle=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_laser")
jointHandle=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_joint")
modelRef=simGetObjectHandle("Hokuyo_URG_04LX_UG01_ROS_ref")
modelHandle=simGetObjectAssociatedWithScript(sim_handle_self)
objName=simGetObjectName(modelHandle)
communicationTube=simTubeOpen(0,objName..'_HOKUYO',1)

scanRange=180*math.pi/180 --You can change the scan range. Angle_min=-scanRange/2, Angle_max=scanRange/2-stepSize
stepSize=2*math.pi/1024
pts=math.floor(scanRange/stepSize)
dists={}
points={}
segments={}
pnts={}
for i=1,pts*3,1 do
table.insert(points,0)
end
for i=1,pts*7,1 do
table.insert(segments,0)
end

black={0,0,0}
red={1,0,0}

-- Check if the required plugin is there (libv_repExtRos.so or libv_repExtRos.dylib):
local moduleName=0
local moduleVersion=0
local index=0
local pluginNotFound=true
while moduleName do
moduleName,moduleVersion=simGetModuleName(index)
if (moduleName=='RosInterface') then
pluginNotFound=false
end
index=index+1
end

if (pluginNotFound) then
else
end
end

if (sim_call_type==sim_childscriptcall_cleanup) then
simRemoveDrawingObject(lines100)
simRemoveDrawingObject(points100)
if pluginNotFound==false then
simExtRosInterface_shutdownPublisher(publisher)
end
end

if (sim_call_type==sim_childscriptcall_sensing) then
showLaserPoints=simGetScriptSimulationParameter(sim_handle_self,'showLaserPoints')
showLaserSegments=simGetScriptSimulationParameter(sim_handle_self,'showLaserSegments')
dists={}
angle=-scanRange*0.5
simSetJointPosition(jointHandle,angle)
jointPos=angle

laserOrigin=simGetObjectPosition(jointHandle,-1)
modelInverseMatrix=simGetInvertedMatrix(simGetObjectMatrix(modelRef,-1))

for ind=0,pts-1,1 do

r,dist,pt=simHandleProximitySensor(laserHandle) -- pt is relative to the laser ray! (rotating!)
m=simGetObjectMatrix(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=simMultiplyVector(m,pt)
ptRelative=simMultiplyVector(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=simMultiplyVector(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
simSetJointPosition(jointHandle,jointPos)
end

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
end
if (showLaserPoints and segments[7*i+7]~=0)then
end
end
end

-- Now send the data:
if #points>0 then
dta=simPackFloats(dists)
lsr = {}
lsr['angle_min']=-scanRange*0.5
lsr['angle_max']=scanRange*0.5-stepSize
lsr['angle_increment']=stepSize
lsr['time_increment']=(1 / 50) / (pts)
lsr['scan_time']=0.02
lsr['range_min']=0
lsr['range_max']=500
lsr['ranges']=simUnpackFloats(dta)
--lsr['ranges']={1,2,3,9,256,175,25,35,58,46}
lsr['intensities']={}
simExtRosInterface_publish(publisher,lsr)
simExtRosInterface_sendTransform(getTransformStamped(modelHandle,objName,-1,'world'))
end
end

Basically, I modified the original script a bit. Ok, it may have some mistakes, but this is not the problem now. The problem is that I am trying to visualise the laser scan data in rviz but I get the following error:

"Transform [sender=unknown_publisher], For frame []: Frame [] does not exist"

Here is a screenshot taken from rviz:

Somewhere I read that I should set the frame id's in the header of the laser scan message, but I am not sure if this is the problem.

Basically I want to visualise my data but it seems I cannot find something by googling. I would really appreciate any help.

Billie1123
Posts: 62
Joined: 08 Jun 2016, 22:47

### Re: VREP, laser scan, Rviz problem

This is more a ROS question than a V-REP question. Next time ask in the ros answers page and you will get better support.
Somewhere I read that I should set the frame id's in the header of the laser scan message, but I am not sure if this is the problem.
Yes, that is basically the problem. The way ROS knows the relative position of each element is using a tf tree. If you run

Code: Select all

$rosrun tf view_frames$ evince frames.pdf
you will see the current relations between each element of your robot. You only seem to have one transform there, between the global frame ("world" or -1 in V-REP) and the laser's frame. That transfrom seems to be properly obtained through "getTransformStamped()" and published through "simExtRosInterface_sendTransform()".

But apart from publishing the transforms, you need to explicitly tell what the frame of the laser data is, otherwise, RViz won't be able to plot the points. You need to fill the header information (at least the frame_id field). So, after "lsr = {}" you can write:

Code: Select all

lsr["header"]={}
lsr.header["frame_id"]=objName
You can look up information in this forum about how to fill the time stamp field if needed. I think the sequence field is filled automatically, but I am not sure. I have not access to ROS right now, so I can't check the code, but with this you should be good to go.

Regards

WillC
Posts: 4
Joined: 23 Apr 2015, 06:33

### Re: VREP, laser scan, Rviz problem

General point: the 'rqt' general tool is a good GUI that includes 'tf view_graph' and a lot more. Just run

Code: Select all

rqt
on the command line, then go to the menu system for a tf tree that you can reload on demand : Plugins -> Visualizations -> TF Tree.