ROS publishing tf info

Report crashes, strange behaviour, or apparent bugs
Post Reply
Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

ROS publishing tf info

Post 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

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: ROS publishing tf info

Post 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

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: ROS publishing tf info

Post 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


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

Re: ROS publishing tf info

Post 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)?

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: ROS publishing tf info

Post 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

Elmo
Posts: 31
Joined: 13 Oct 2016, 16:34

Re: ROS publishing tf info

Post 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

Post Reply