Coordinate transformation after storing all tf messages from a rosbag
Hi, After storing all tf messages in a list from a rosbag file, I am trying to do coordinate transformations at specific times in a loop. I have target times for the transformations to occur and I assume that transform function picks a closest tf message at each target time. But, the output doesn't look correct. Can someone please help me how to fix the code below?
rosbag_file = rosbag.Bag(rosbag_dir, 'r')
static_transform_list = []
for topic, msg, t in rosbag_file.read_messages(topics=['/tf_static']):
for static_transform in msg.transforms:
static_transform_list.append(static_transform)
transform_list = []
for topic, msg, t in rosbag_file.read_messages(topics=['/tf']):
for transform in msg.transforms:
transform_list.append(transform)
tf2_transformer = tf2_ros.Buffer()
for static_transform in static_transform_list:
tf2_transformer.set_transform_static(static_transform, '')
for transform in transform_list:
tf2_transformer.set_transform(transform, '')
for frame_t, p in enumerate(frame_t_list):
pmsg = geometry_msgs.msg.PointStamped()
pmsg.header.frame_id = 'base'
pmsg.header.stamp = rospy.Time.from_sec(frame_t)
pmsg.point.x = p[0]
pmsg.point.y = p[1]
pmsg.point.z = p[2]
pmsg_world = tf2_transformer.transform(pmsg, target_frame='world')
Please be more specific than "the output doesn't look correct". What is wrong with the output? What were you expecting?
Please post your entire code and the bag file so we can try it for ourselves.
TF may interpolate between known poses if you ask it for a transform that is between tf messages.
Hi Geoff,
Thank you very much for your reply. I am sorry that I couldn't be specific enough. Regarding your last comment, I was wondering if the tf2_transformer.transform(pmsg, target_frame='world') picks up a transformation at pmsg.header.stamp. If a transformation is not available at the exact target time which is pmsg.header.stamp, does it automatically interpolate between two tf messages which are close to the target time?