VREP, laser scan, Rviz problem

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

VREP, laser scan, Rviz problem

Post by makbut » 29 Dec 2016, 12:08

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 {
        header={
            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}
    lines100=simAddDrawingObject(sim_drawing_lines,1,0,-1,1000,black,black,black,red)
    points100=simAddDrawingObject(sim_drawing_points,4,0,-1,1000,black,black,black,red)


    -- 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
        -- Display an error message if the plugin was not found:
        simDisplayDialog('Hokuyo_URG_04LX_UG01_ROS error','ROS plugin was not found.&&nSimulation will not run properly',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    else
        publisher=simExtRosInterface_advertise('/las_scn', 'sensor_msgs/LaserScan')
    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
  
    
    simAddDrawingObjectItem(lines100,nil)
    simAddDrawingObjectItem(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
                simAddDrawingObjectItem(lines100,t)
            end
            if (showLaserPoints and segments[7*i+7]~=0)then
                simAddDrawingObjectItem(points100,t)
            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:

Image

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.
Thank you in advance

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

Re: VREP, laser scan, Rviz problem

Post by Billie1123 » 29 Dec 2016, 13:40

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

Post by WillC » 18 Mar 2018, 11:37

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.

Post Reply