tf_conversions / posemath / pykdl problem with fromMsg
There seems to be a bug in the python kdl wrapper, leading to unexpected behavior when working on KDL frames. I felt like posting it here since other people might run into it before it's fixed, and they would most probably look for a solution here.
The simple remedy is of course to assign whatever is returned from fromMsg to a variable before usage.
import roslib; roslib.load_manifest('tf')
roslib.load_manifest('tf_conversions')
import tf_conversions.posemath as pm
roslib.load_manifest('geometry_msgs')
import geometry_msgs
pose_msg = geometry_msgs.msg.Pose()
pose_msg.position.x = 100
print pm.fromMsg(pose_msg).p
#[ 0, 0, 0]
pose = pm.fromMsg(pose_msg)
print pose.p
#[ 100, 0, 0]
We can create this error using kdl: import PyKDL as kdl kdl.Frame(kdl.Rotation.Quaternion(0,0,0,1),kdl.Vector(100,200,300)).p Out[9]: [2.56735e-316,6.93179e-310, 300] kdl.Frame(kdl.Rotation.Quaternion(0,0,0,1),kdl.Vector(100,200,300)).p[0] Out[10]: 0.0 # which is not 100!
Turns out it's a general problem with SIP. Whenever we have a struct C { Member m; } and access from from a an instantiation C().m, the instantiated object gets garbage collected while .m still points to the memory held by C++ directly. It seems like there is no way to do this right in SIP.