加入loam算法

sensing/slam/laser_slam/script/data_transfer.py

@@ -1,7 +1,6 @@
 import rospy
 from gazebo_msgs.srv import GetModelState
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import Pose2D
+from geometry_msgs.msg import PoseStamped, Pose2D
 from nav_msgs.msg import Odometry
 import tf
 import sys
@@ -10,20 +9,26 @@ rospy.init_node('laser_slam_data_transfer')
 pose_pub = rospy.Publisher("/mavros/vision_pose/pose", PoseStamped, queue_size=2)
 local_pose = PoseStamped()
 local_pose.header.frame_id = 'map'
-rate = rospy.Rate(100)
-local_pose = PoseStamped()
 laser_scan = Pose2D()
 
 def odm_groundtruth_callback(msg):
-    local_pose.header = msg.header
+    global local_pose
+    local_pose.header.stamp = msg.header.stamp
     local_pose.pose.position.z  = msg.pose.pose.position.z
+    
+def odm_aloam_callback(msg):
+    global local_pose
+    local_pose.header.stamp = msg.header.stamp
+    local_pose.pose = msg.pose.pose
 
 def callback(data):
     global laser_scan
     laser_scan = data
     
 def laser_scan_matcher():
+    global local_pose
     pose2d_sub = rospy.Subscriber("/pose2D", Pose2D, callback)
+    rate = rospy.Rate(100)
     while True:
         local_pose.header.stamp = rospy.Time.now()
         local_pose.pose.position.x = laser_scan.x
@@ -37,6 +42,8 @@ def laser_scan_matcher():
 
 def cartographer2D():
     listener = tf.TransformListener()
+    global local_pose
+    rate = rospy.Rate(100)
     while True:
         try:
             translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
@@ -51,6 +58,8 @@ def cartographer2D():
         
 def cartographer3D():
     listener = tf.TransformListener()
+    global local_pose
+    rate = rospy.Rate(100)
     while True:
         try:
             translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
@@ -65,6 +74,13 @@ def cartographer3D():
         local_pose.pose.orientation.z = rotation[3]
         pose_pub.publish(local_pose)
         rate.sleep()
+        
+def aloam():
+    global local_pose
+    rate = rospy.Rate(100)
+    while True:
+        pose_pub.publish(local_pose)
+        rate.sleep()
     
 if __name__ == '__main__':
     try:
@@ -79,6 +95,9 @@ if __name__ == '__main__':
         cartographer2D()
     elif laser_slam_type == 'cartographer3D':
         cartographer3D()
+    elif laser_slam_type == 'aloam':
+        odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
+        aloam()
     else:
         print('You should choose from laser_scan_matcher, cartographer2D and cartographer3D.')