加入loam算法
This commit is contained in:
parent
097f41067a
commit
9ae0628d69
|
@ -1,7 +1,6 @@
|
||||||
import rospy
|
import rospy
|
||||||
from gazebo_msgs.srv import GetModelState
|
from gazebo_msgs.srv import GetModelState
|
||||||
from geometry_msgs.msg import PoseStamped
|
from geometry_msgs.msg import PoseStamped, Pose2D
|
||||||
from geometry_msgs.msg import Pose2D
|
|
||||||
from nav_msgs.msg import Odometry
|
from nav_msgs.msg import Odometry
|
||||||
import tf
|
import tf
|
||||||
import sys
|
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)
|
pose_pub = rospy.Publisher("/mavros/vision_pose/pose", PoseStamped, queue_size=2)
|
||||||
local_pose = PoseStamped()
|
local_pose = PoseStamped()
|
||||||
local_pose.header.frame_id = 'map'
|
local_pose.header.frame_id = 'map'
|
||||||
rate = rospy.Rate(100)
|
|
||||||
local_pose = PoseStamped()
|
|
||||||
laser_scan = Pose2D()
|
laser_scan = Pose2D()
|
||||||
|
|
||||||
def odm_groundtruth_callback(msg):
|
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
|
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):
|
def callback(data):
|
||||||
global laser_scan
|
global laser_scan
|
||||||
laser_scan = data
|
laser_scan = data
|
||||||
|
|
||||||
def laser_scan_matcher():
|
def laser_scan_matcher():
|
||||||
|
global local_pose
|
||||||
pose2d_sub = rospy.Subscriber("/pose2D", Pose2D, callback)
|
pose2d_sub = rospy.Subscriber("/pose2D", Pose2D, callback)
|
||||||
|
rate = rospy.Rate(100)
|
||||||
while True:
|
while True:
|
||||||
local_pose.header.stamp = rospy.Time.now()
|
local_pose.header.stamp = rospy.Time.now()
|
||||||
local_pose.pose.position.x = laser_scan.x
|
local_pose.pose.position.x = laser_scan.x
|
||||||
|
@ -37,6 +42,8 @@ def laser_scan_matcher():
|
||||||
|
|
||||||
def cartographer2D():
|
def cartographer2D():
|
||||||
listener = tf.TransformListener()
|
listener = tf.TransformListener()
|
||||||
|
global local_pose
|
||||||
|
rate = rospy.Rate(100)
|
||||||
while True:
|
while True:
|
||||||
try:
|
try:
|
||||||
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
|
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
|
||||||
|
@ -51,6 +58,8 @@ def cartographer2D():
|
||||||
|
|
||||||
def cartographer3D():
|
def cartographer3D():
|
||||||
listener = tf.TransformListener()
|
listener = tf.TransformListener()
|
||||||
|
global local_pose
|
||||||
|
rate = rospy.Rate(100)
|
||||||
while True:
|
while True:
|
||||||
try:
|
try:
|
||||||
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
|
translation,rotation = listener.lookupTransform("base_link","odom",rospy.Time())
|
||||||
|
@ -66,6 +75,13 @@ def cartographer3D():
|
||||||
pose_pub.publish(local_pose)
|
pose_pub.publish(local_pose)
|
||||||
rate.sleep()
|
rate.sleep()
|
||||||
|
|
||||||
|
def aloam():
|
||||||
|
global local_pose
|
||||||
|
rate = rospy.Rate(100)
|
||||||
|
while True:
|
||||||
|
pose_pub.publish(local_pose)
|
||||||
|
rate.sleep()
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
try:
|
try:
|
||||||
laser_slam_type = sys.argv[1]
|
laser_slam_type = sys.argv[1]
|
||||||
|
@ -79,6 +95,9 @@ if __name__ == '__main__':
|
||||||
cartographer2D()
|
cartographer2D()
|
||||||
elif laser_slam_type == 'cartographer3D':
|
elif laser_slam_type == 'cartographer3D':
|
||||||
cartographer3D()
|
cartographer3D()
|
||||||
|
elif laser_slam_type == 'aloam':
|
||||||
|
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
|
||||||
|
aloam()
|
||||||
else:
|
else:
|
||||||
print('You should choose from laser_scan_matcher, cartographer2D and cartographer3D.')
|
print('You should choose from laser_scan_matcher, cartographer2D and cartographer3D.')
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue