gt_tf_broadcaster.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib
3 import rospy
4 import os
5 import tf
6 import numpy
7 
8 if __name__ == '__main__':
9  rospy.init_node('groundtruth_tf_broadcaster')
10  fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
11  baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
12  offset_time = rospy.get_param('~offset_time', 0.0)
13  offset_x = rospy.get_param('~offset_x', 0.0)
14  offset_y = rospy.get_param('~offset_y', 0.0)
15  offset_theta = rospy.get_param('~offset_theta', 0.0)
16  gtFile = rospy.get_param('~file', 'groundtruth.txt')
17  gtFile = os.path.expanduser(gtFile)
19 
20  init_x = 0
21  init_y = 0
22  init = False
23 
24  # assuming format "timestamp,x,y,theta"
25  for line in open(gtFile,'r'):
26  mylist = line.split(',')
27  if len(mylist) == 4 and not rospy.is_shutdown():
28  stamp = float(int(mylist[0]))/1000000.0 + offset_time
29  x = float(mylist[1])
30  y = float(mylist[2])
31  theta = float(mylist[3])
32 
33  if not init:
34  init_x = x
35  init_y = y
36  init = True
37 
38  x -= init_x
39  y -= init_y
40 
41  trans1_mat = tf.transformations.translation_matrix((x, y, 0))
42  rot1_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, theta))
43  mat1 = numpy.dot(trans1_mat, rot1_mat)
44 
45  trans2_mat = tf.transformations.translation_matrix((offset_x, offset_y, 0))
46  rot2_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, offset_theta))
47  mat2 = numpy.dot(trans2_mat, rot2_mat)
48 
49  mat3 = numpy.dot(mat1, mat2)
50  trans3 = tf.transformations.translation_from_matrix(mat3)
51  rot3 = tf.transformations.quaternion_from_matrix(mat3)
52 
53  #print(mat1)
54  #print(mat2)
55  #print(mat3)
56 
57  now = rospy.get_time()
58  while not rospy.is_shutdown() and now < stamp:
59  delay = stamp - now
60  if delay > 0.05:
61  delay = 0.05
62  rospy.sleep(delay)
63  now = rospy.get_time()
64  if not rospy.is_shutdown():
65  br.sendTransform(trans3,
66  rot3,
67  rospy.Time.from_sec(stamp),
68  baseFrame,
69  fixedFrame)
70  else:
71  break


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40