|
| | gt_tf_broadcaster.baseFrame = rospy.get_param('~frame_id', 'base_link_gt') |
| |
| | gt_tf_broadcaster.br = tf.TransformBroadcaster() |
| |
| | gt_tf_broadcaster.delay = stamp-now |
| |
| | gt_tf_broadcaster.fixedFrame = rospy.get_param('~fixed_frame_id', 'world') |
| |
| | gt_tf_broadcaster.gtFile = rospy.get_param('~file', 'groundtruth.txt') |
| |
| bool | gt_tf_broadcaster.init = False |
| |
| int | gt_tf_broadcaster.init_x = 0 |
| |
| int | gt_tf_broadcaster.init_y = 0 |
| |
| | gt_tf_broadcaster.mat1 = numpy.dot(trans1_mat, rot1_mat) |
| |
| | gt_tf_broadcaster.mat2 = numpy.dot(trans2_mat, rot2_mat) |
| |
| | gt_tf_broadcaster.mat3 = numpy.dot(mat1, mat2) |
| |
| | gt_tf_broadcaster.mylist = line.split(',') |
| |
| | gt_tf_broadcaster.now = rospy.get_time() |
| |
| | gt_tf_broadcaster.offset_theta = rospy.get_param('~offset_theta', 0.0) |
| |
| | gt_tf_broadcaster.offset_time = rospy.get_param('~offset_time', 0.0) |
| |
| | gt_tf_broadcaster.offset_x = rospy.get_param('~offset_x', 0.0) |
| |
| | gt_tf_broadcaster.offset_y = rospy.get_param('~offset_y', 0.0) |
| |
| | gt_tf_broadcaster.rot1_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, theta)) |
| |
| | gt_tf_broadcaster.rot2_mat = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, offset_theta)) |
| |
| | gt_tf_broadcaster.rot3 = tf.transformations.quaternion_from_matrix(mat3) |
| |
| float | gt_tf_broadcaster.stamp = float(int(mylist[0]))/1000000.0 |
| |
| | gt_tf_broadcaster.theta = float(mylist[3]) |
| |
| | gt_tf_broadcaster.trans1_mat = tf.transformations.translation_matrix((x, y, 0)) |
| |
| | gt_tf_broadcaster.trans2_mat = tf.transformations.translation_matrix((offset_x, offset_y, 0)) |
| |
| | gt_tf_broadcaster.trans3 = tf.transformations.translation_from_matrix(mat3) |
| |
| | gt_tf_broadcaster.x = float(mylist[1]) |
| |
| | gt_tf_broadcaster.y = float(mylist[2]) |
| |