Refactor transform fusion to individual module

scripts/transform_fusion.py

@@ -0,0 +1,91 @@
+#!/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()