I'd like to transform a `PointStamped` using a python `tf2_ros.Buffer`, but so far I'm only getting type exceptions.
The following example a point with only a y component is supposed to be trivially transformed into the frame it is already in:
#!/usr/bin/env python
import rospy
import sys
import tf2_ros
from geometry_msgs.msg import PointStamped
if __name__ == '__main__':
rospy.init_node('transform_point_tf2')
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
rospy.sleep(1.0)
pt = PointStamped()
pt.header.stamp = rospy.Time.now()
pt.header.frame_id = "map"
pt.point.x = 0.0
pt.point.y = 1.0
pt.point.z = 0.0
try:
pt2 = tf_buffer.transform(pt, "map")
except: # tf2_ros.buffer_interface.TypeException as e:
e = sys.exc_info()[0]
rospy.logerr(e)
sys.exit(1)
rospy.loginfo(pt2)
This results in:
[ERROR] [/transform_point_tf2] [./transform_point_tf2.py]:[27] []
If PointStamped isn't the right type, then what type can I use?
To do this manually I'm currently doing this:
trans = self.tf_buffer.lookup_transform("map", target_frame,
rospy.Time())
quat = [trans.transform.rotation.x,
trans.transform.rotation.y,
trans.transform.rotation.z,
trans.transform.rotation.w]
mat = tf.transformations.quaternion_matrix(quat)
pt_np = [pt.point.x,
pt.point.y,
pt.point.z,
1.0]
pt_in_map_np = numpy.dot(mat, pt_np)
pt_in_map.x = pt_in_map_np[0]
pt_in_map.y = pt_in_map_np[1]
pt_in_map.z = pt_in_map_np[2]
↧