forked from xtdrone/XTDrone
modify laser_transfer
This commit is contained in:
parent
0bd55ee531
commit
8e20b59b78
|
|
@ -2,7 +2,7 @@ import rospy
|
|||
from gazebo_msgs.srv import GetModelState
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from nav_msgs.msg import Odometry
|
||||
import tf
|
||||
from tf2_ros import TransformListener, Buffer
|
||||
import sys
|
||||
|
||||
vehicle_type = sys.argv[1]
|
||||
|
|
@ -10,6 +10,8 @@ vehicle_id = sys.argv[2]
|
|||
laser_slam_type = sys.argv[3]
|
||||
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
|
||||
pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=2)
|
||||
tfBuffer = Buffer()
|
||||
tflistener = TransformListener(tfBuffer)
|
||||
local_pose = PoseStamped()
|
||||
hector = PoseStamped()
|
||||
height = 0
|
||||
|
|
@ -18,11 +20,6 @@ def odm_groundtruth_callback(msg):
|
|||
global height
|
||||
height = 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 hector_callback(data):
|
||||
global hector
|
||||
hector = data
|
||||
|
|
@ -39,18 +36,24 @@ def hector_slam():
|
|||
|
||||
def aloam():
|
||||
global local_pose
|
||||
rate = rospy.Rate(100)
|
||||
while True:
|
||||
rate = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
|
||||
except:
|
||||
continue
|
||||
local_pose.header.stamp = rospy.Time().now()
|
||||
local_pose.header.frame_id = 'map'
|
||||
local_pose.pose.position = tfstamped.transform.translation
|
||||
local_pose.pose.orientation = tfstamped.transform.rotation
|
||||
pose_pub.publish(local_pose)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
if laser_slam_type == '2d':
|
||||
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
|
||||
hector_slam()
|
||||
elif laser_slam_type == '3d':
|
||||
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
|
||||
aloam()
|
||||
else:
|
||||
print('input error')
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@ import rospy
|
|||
from gazebo_msgs.srv import GetModelState
|
||||
from geometry_msgs.msg import PoseStamped, Pose2D
|
||||
from nav_msgs.msg import Odometry
|
||||
import tf
|
||||
from tf2_ros import TransformListener, Buffer
|
||||
import sys
|
||||
|
||||
vehicle_type = sys.argv[1]
|
||||
|
|
@ -19,11 +19,6 @@ def odm_groundtruth_callback(msg):
|
|||
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 laser_scan_matcher_callback(data):
|
||||
global laser_scan
|
||||
laser_scan = data
|
||||
|
|
@ -46,8 +41,16 @@ def laser_scan_matcher():
|
|||
|
||||
def aloam():
|
||||
global local_pose
|
||||
rate = rospy.Rate(100)
|
||||
while True:
|
||||
rate = rospy.Rate(30)
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
|
||||
except:
|
||||
continue
|
||||
local_pose.header.stamp = rospy.Time().now()
|
||||
local_pose.header.frame_id = 'map'
|
||||
local_pose.pose.position = tfstamped.transform.translation
|
||||
local_pose.pose.orientation = tfstamped.transform.rotation
|
||||
pose_pub.publish(local_pose)
|
||||
rate.sleep()
|
||||
|
||||
|
|
@ -57,7 +60,6 @@ if __name__ == '__main__':
|
|||
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+ vehicle_id+'/ground_truth/odom', Odometry, odm_groundtruth_callback)
|
||||
laser_scan_matcher()
|
||||
elif laser_slam_type == '3d':
|
||||
odom_aloam_sub = rospy.Subscriber('/laser_odom_to_init', Odometry, odm_aloam_callback)
|
||||
aloam()
|
||||
else:
|
||||
print('input error')
|
||||
|
|
|
|||
|
|
@ -773,27 +773,12 @@
|
|||
<light name='user_point_light_1'>
|
||||
<pose frame=''>0.766851 -12.0385 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_2'>
|
||||
<pose frame=''>-2.25143 -7.57347 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_3'>
|
||||
<pose frame=''>2.80305 -7.33891 1.86283 -0.49865 0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_4'>
|
||||
<pose frame=''>0.076075 -5.91752 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_5'>
|
||||
<pose frame=''>-4.36942 -10.4942 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_6'>
|
||||
<pose frame=''>-3.92433 -9.26622 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_7'>
|
||||
<pose frame=''>-3.73468 -12.8798 1 0 -0 0</pose>
|
||||
</light>
|
||||
<light name='user_point_light_8'>
|
||||
<pose frame=''>-3.8302 -8.15565 1 0 -0 0</pose>
|
||||
</light>
|
||||
</state>
|
||||
<model name='cafe_table'>
|
||||
<static>1</static>
|
||||
|
|
@ -1556,19 +1541,6 @@
|
|||
</attenuation>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</light>
|
||||
<light name='user_point_light_2' type='point'>
|
||||
<pose frame=''>-2.25143 -7.57347 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>20</range>
|
||||
<constant>0.5</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<light name='user_point_light_3' type='point'>
|
||||
<pose frame=''>2.80305 -7.84683 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
|
|
@ -1621,58 +1593,6 @@
|
|||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<light name='user_point_light_5' type='point'>
|
||||
<pose frame=''>-4.36942 -10.4942 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>20</range>
|
||||
<constant>0.5</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<light name='user_point_light_6' type='point'>
|
||||
<pose frame=''>-3.92433 -9.26622 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>20</range>
|
||||
<constant>0.5</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<light name='user_point_light_7' type='point'>
|
||||
<pose frame=''>-3.73468 -12.8798 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>20</range>
|
||||
<constant>0.5</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<light name='user_point_light_8' type='point'>
|
||||
<pose frame=''>-3.8302 -8.15565 1 0 -0 0</pose>
|
||||
<diffuse>0.5 0.5 0.5 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<attenuation>
|
||||
<range>20</range>
|
||||
<constant>0.5</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<direction>0 0 -1</direction>
|
||||
</light>
|
||||
<model name='bookshelf'>
|
||||
<static>1</static>
|
||||
<link name='link'>
|
||||
|
|
|
|||
Loading…
Reference in New Issue