ROS publishing tf info
Posted: 11 Nov 2016, 23:47
V-REP inverse all tf-values, by sending them. Is that normal?
embedded Skript(lua)
sending from V-REP:
receiving in Python:
Python Code
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)
Code: Select all
0.74999976158142 -1.5582273249493e-08 0.14999997615814
Code: Select all
pos: (-0.7499997515119277, 1.0343387636527552e-07, -0.1500000265055559)
orien: (-2.220827077792358e-08, -3.356495170692133e-08, -6.415329778519644e-08, 0.9999999999999971)
Code: Select all
listener = TransformListener()
t = listener.getLatestCommonTime(boxHandle, relTo)
pos, orien = listener.lookupTransform(boxHandle, relTo, t)
print boxHandle,': \npos:',pos,'\norien:',orien