Page 1 of 1

ROS publishing tf info

Posted: 11 Nov 2016, 23:47
by Elmo
V-REP inverse all tf-values, by sending them. Is that normal?

embedded Skript(lua)

Code: Select all

-- function for tf publications
function getTransformStamped(objHandle,name,relTo,relToName)
    t=simGetSystemTime()
    p=simGetObjectPosition(objHandle,relTo)
    o=simGetObjectQuaternion(objHandle,relTo)
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            -- ROS has definition x=front y=side z=up
            translation={x=p[1],y=p[2],z=p[3]},--V-rep
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}--v-rep
        }
    }
end


         -- To send several transforms at once, use simExtRosInterface_sendTransforms insteads
        transformations = {}
        transformations[1]=(getTransformStamped(vehicleReference,'base_link',-1,'map'))
        transformations[2]=(getTransformStamped(wheelJoints[1],'wheel_link_fl',vehicleReference,'base_link'))
        transformations[3]=(getTransformStamped(wheelJoints[2],'wheel_link_bl',vehicleReference,'base_link'))
        transformations[4]=(getTransformStamped(wheelJoints[3],'wheel_link_br',vehicleReference,'base_link'))
        transformations[5]=(getTransformStamped(wheelJoints[4],'wheel_link_fr',vehicleReference,'base_link'))

        transformations[6]=(getTransformStamped(armJoints[1],'arm_link_0',vehicleReference,'base_link'))
        transformations[7]=(getTransformStamped(armJoints[2],'arm_link_1',armJoints[1],'arm_link_0'))
        transformations[8]=(getTransformStamped(armJoints[3],'arm_link_2',armJoints[2],'arm_link_1'))
        transformations[9]=(getTransformStamped(armJoints[4],'arm_link_3',armJoints[3],'arm_link_2'))
        transformations[10]=(getTransformStamped(armJoints[5],'arm_link_4',armJoints[4],'arm_link_3'))
        
        simExtRosInterface_sendTransforms(transformations)


sending from V-REP:

Code: Select all

0.74999976158142     -1.5582273249493e-08     0.14999997615814
receiving in Python:

Code: Select all

pos: (-0.7499997515119277, 1.0343387636527552e-07, -0.1500000265055559) 
orien: (-2.220827077792358e-08, -3.356495170692133e-08, -6.415329778519644e-08, 0.9999999999999971)
Python Code

Code: Select all

     listener = TransformListener()
    t = listener.getLatestCommonTime(boxHandle, relTo)
    pos, orien = listener.lookupTransform(boxHandle, relTo, t)
    print boxHandle,': \npos:',pos,'\norien:',orien

Re: ROS publishing tf info

Posted: 13 Nov 2016, 18:59
by coppelia
Hello,

no, that is not normal, you must be doing something wrong. What exactly are you printing in V-REP and how/when?

Cheers

Re: ROS publishing tf info

Posted: 14 Nov 2016, 13:04
by Elmo
Directly in getTransformStamped
but its also strange, that I do receive all negativ. Looking on the scene the red box of the Hanoi example is in pos.dirextion of x.

BoxTF_red1 position: 0.74999976158142 1.5959550347944e-09 0.14999997615814

Code: Select all

function getTransformStamped(objHandle,name,relTo,relToName)
    t=simGetSystemTime()
    p=simGetObjectPosition(objHandle,relTo)
    o=simGetObjectQuaternion(objHandle,relTo)

    simAuxiliaryConsolePrint(DebugConsole,name)
    simAuxiliaryConsolePrint(DebugConsole," position: ")
    simAuxiliaryConsolePrint(DebugConsole,p[1])
    simAuxiliaryConsolePrint(DebugConsole,' ')
    simAuxiliaryConsolePrint(DebugConsole,p[2])
    simAuxiliaryConsolePrint(DebugConsole,' ')
    simAuxiliaryConsolePrint(DebugConsole,p[3])
    simAuxiliaryConsolePrint(DebugConsole,'\n')

    simAuxiliaryConsolePrint(DebugConsole,name)
    simAuxiliaryConsolePrint(DebugConsole," orientattion: ")
    simAuxiliaryConsolePrint(DebugConsole,o[1])
    simAuxiliaryConsolePrint(DebugConsole,' ')
    simAuxiliaryConsolePrint(DebugConsole,o[2])
    simAuxiliaryConsolePrint(DebugConsole,' ')
    simAuxiliaryConsolePrint(DebugConsole,o[3])
    simAuxiliaryConsolePrint(DebugConsole,'\n')
    
    return {
        header={
            stamp=t,
            frame_id=relToName
        },
        child_frame_id=name,
        transform={
            -- ROS has definition x=front y=side z=up
            translation={x=p[1],y=p[2],z=p[3]},--V-rep
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}--v-rep
        }
    }
end


Re: ROS publishing tf info

Posted: 16 Nov 2016, 14:56
by fferri
That sounds very strange.

There is no code for explicitly inverting those values in the RosInterface plugin. All messages are composed of other messages or basic types (int, float, string), and the TransformStamped message is no exception.

Try to display the TF tree in R-Viz: does it appear incorrect (i.e. different from the V-REP)? Can you post a screenshot?
Be sure to align the axes consistently between V-REP and R-Viz (when you select the top-XY ortho view in R-Viz, you need to rotate it 180 degrees to match the view in V-REP). I did a quick test with your code and the MTB robot, and everything looks OK:

Image

Also, can you please post a complete reproducible test case (i.e. the V-REP scene file, the python script, and anything else that is needed to reproduce this issue)?

Re: ROS publishing tf info

Posted: 17 Nov 2016, 13:41
by Elmo
Thank you so much fferi for you response.
indeed, in RVIZ it looks normal! see pictures. Thats why I didn't realized it in first place
But I have to send the inverse back to V-REP too, to move into the right direction.
Could it be that I have to publish a /map coordinates? That RVIZ has one and in Python not?



Image
Image

Re: ROS publishing tf info

Posted: 20 Nov 2016, 15:14
by Elmo
I tried with
rostopic echo /tf and it works fine. So my listener was receiving wrong datas.

had to change the parameters in: pos, orien = listener.lookupTransform(relTo, boxHandle, t)
So everything works fine. Sorry for that false alarm

Code: Select all

    header: 
      seq: 0
      stamp: 
        secs: 2182328
        nsecs: 550000000
      frame_id: map
    child_frame_id: BoxTF_red1
    transform: 
      translation: 
        x: 0.749999761581
        y: -4.91876372966e-10
        z: 0.149999976158
      rotation: 
        x: -7.47471112783e-11
        y: 4.05464239961e-09
        z: 1.34871584123e-07
        w: 1.0