加入loam算法
This commit is contained in:
parent
097f41067a
commit
9ae0628d69
|
@ -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.')
|
||||
|
||||
|
|
Loading…
Reference in New Issue