mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
72 lines
2.4 KiB
Python
72 lines
2.4 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
import roslib
|
||
|
|
import rospy
|
||
|
|
import os
|
||
|
|
import tf
|
||
|
|
import numpy
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
rospy.init_node('groundtruth_tf_broadcaster')
|
||
|
|
fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
|
||
|
|
baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
|
||
|
|
offset_time = rospy.get_param('~offset_time', 0.0)
|
||
|
|
offset_x = rospy.get_param('~offset_x', 0.0)
|
||
|
|
offset_y = rospy.get_param('~offset_y', 0.0)
|
||
|
|
offset_theta = rospy.get_param('~offset_theta', 0.0)
|
||
|
|
gtFile = rospy.get_param('~file', 'groundtruth.txt')
|
||
|
|
gtFile = os.path.expanduser(gtFile)
|
||
|
|
br = tf.TransformBroadcaster()
|
||
|
|
|
||
|
|
init_x = 0
|
||
|
|
init_y = 0
|
||
|
|
init = False
|
||
|
|
|
||
|
|
# assuming format "timestamp,x,y,theta"
|
||
|
|
for line in open(gtFile,'r'):
|
||
|
|
mylist = line.split(',')
|
||
|
|
if len(mylist) == 4 and not rospy.is_shutdown():
|
||
|
|
stamp = float(int(mylist[0]))/1000000.0 + offset_time
|
||
|
|
x = float(mylist[1])
|
||
|
|
y = float(mylist[2])
|
||
|
|
theta = float(mylist[3])
|
||
|
|
|
||
|
|
if not init:
|
||
|
|
init_x = x
|
||
|
|
init_y = y
|
||
|
|
init = True
|
||
|
|
|
||
|
|
x -= init_x
|
||
|
|
y -= init_y
|
||
|
|
|
||
|
|
trans1_mat = tf.transformations.translation_matrix((x, y, 0))
|
||
|
|
rot1_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, theta))
|
||
|
|
mat1 = numpy.dot(trans1_mat, rot1_mat)
|
||
|
|
|
||
|
|
trans2_mat = tf.transformations.translation_matrix((offset_x, offset_y, 0))
|
||
|
|
rot2_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, offset_theta))
|
||
|
|
mat2 = numpy.dot(trans2_mat, rot2_mat)
|
||
|
|
|
||
|
|
mat3 = numpy.dot(mat1, mat2)
|
||
|
|
trans3 = tf.transformations.translation_from_matrix(mat3)
|
||
|
|
rot3 = tf.transformations.quaternion_from_matrix(mat3)
|
||
|
|
|
||
|
|
#print(mat1)
|
||
|
|
#print(mat2)
|
||
|
|
#print(mat3)
|
||
|
|
|
||
|
|
now = rospy.get_time()
|
||
|
|
while not rospy.is_shutdown() and now < stamp:
|
||
|
|
delay = stamp - now
|
||
|
|
if delay > 0.05:
|
||
|
|
delay = 0.05
|
||
|
|
rospy.sleep(delay)
|
||
|
|
now = rospy.get_time()
|
||
|
|
if not rospy.is_shutdown():
|
||
|
|
br.sendTransform(trans3,
|
||
|
|
rot3,
|
||
|
|
rospy.Time.from_sec(stamp),
|
||
|
|
baseFrame,
|
||
|
|
fixedFrame)
|
||
|
|
else:
|
||
|
|
break
|