mirror of
https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
synced 2023-05-28 12:51:38 +08:00
92 lines
2.9 KiB
Python
Executable File
92 lines
2.9 KiB
Python
Executable File
#!/usr/bin/env python2
|
|
# coding=utf8
|
|
from __future__ import print_function, division, absolute_import
|
|
|
|
import copy
|
|
import thread
|
|
import time
|
|
|
|
import pcl
|
|
import rospy
|
|
import ros_numpy
|
|
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion
|
|
from nav_msgs.msg import Odometry
|
|
from sensor_msgs.msg import PointCloud2
|
|
import numpy as np
|
|
import tf
|
|
import tf.transformations
|
|
|
|
cur_odom_to_baselink = None
|
|
cur_map_to_odom = None
|
|
|
|
|
|
def pose_to_mat(pose_msg):
|
|
return np.matmul(
|
|
tf.listener.xyz_to_mat44(pose_msg.pose.pose.position),
|
|
tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation),
|
|
)
|
|
|
|
|
|
def transform_fusion():
|
|
global cur_odom_to_baselink, cur_map_to_odom
|
|
|
|
br = tf.TransformBroadcaster()
|
|
while True:
|
|
time.sleep(1 / FREQ_PUB_LOCALIZATION)
|
|
|
|
# TODO 这里注意线程安全
|
|
cur_odom = copy.copy(cur_odom_to_baselink)
|
|
if cur_map_to_odom is not None:
|
|
T_map_to_odom = pose_to_mat(cur_map_to_odom)
|
|
else:
|
|
T_map_to_odom = np.eye(4)
|
|
|
|
br.sendTransform(tf.transformations.translation_from_matrix(T_map_to_odom),
|
|
tf.transformations.quaternion_from_matrix(T_map_to_odom),
|
|
rospy.Time.now(),
|
|
'camera_init', 'map')
|
|
if cur_odom is not None:
|
|
# 发布全局定位的odometry
|
|
localization = Odometry()
|
|
T_odom_to_base_link = pose_to_mat(cur_odom)
|
|
# 这里T_map_to_odom短时间内变化缓慢 暂时不考虑与T_odom_to_base_link时间同步
|
|
T_map_to_base_link = np.matmul(T_map_to_odom, T_odom_to_base_link)
|
|
xyz = tf.transformations.translation_from_matrix(T_map_to_base_link)
|
|
quat = tf.transformations.quaternion_from_matrix(T_map_to_base_link)
|
|
localization.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
|
|
localization.twist = cur_odom.twist
|
|
|
|
localization.header.stamp = cur_odom.header.stamp
|
|
localization.header.frame_id = 'map'
|
|
localization.child_frame_id = 'body'
|
|
# rospy.loginfo_throttle(1, '{}'.format(np.matmul(T_map_to_odom, T_odom_to_base_link)))
|
|
pub_localization.publish(localization)
|
|
|
|
|
|
def cb_save_cur_odom(odom_msg):
|
|
global cur_odom_to_baselink
|
|
cur_odom_to_baselink = odom_msg
|
|
|
|
|
|
def cb_save_map_to_odom(odom_msg):
|
|
global cur_map_to_odom
|
|
cur_map_to_odom = odom_msg
|
|
|
|
|
|
if __name__ == '__main__':
|
|
# tf and localization publishing frequency (HZ)
|
|
FREQ_PUB_LOCALIZATION = 50
|
|
|
|
rospy.init_node('transform_fusion')
|
|
rospy.loginfo('Transform Fusion Node Inited...')
|
|
|
|
rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1)
|
|
rospy.Subscriber('/map_to_odom', Odometry, cb_save_map_to_odom, queue_size=1)
|
|
|
|
pub_localization = rospy.Publisher('/localization', Odometry, queue_size=1)
|
|
|
|
# 发布定位消息
|
|
thread.start_new_thread(transform_fusion, ())
|
|
|
|
rospy.spin()
|