Airsim通过ros发布激光雷达数据+Lego-loam仿真测试(2)

上篇博客只是简单跑通了流程，存在的问题将在这篇进行修正。

一、Lego-loam里话题订阅

雷达点云话题为：/velodyne_points，frame_id为velodyne

IMU话题为：/Imu/data，可选

我只用了雷达点云信息，slam效果不错。在airsim里发布激光雷达点云的代码如下：

#!/usr/bin/env python3
#-*- coding:utf-8 -*-

import math
import rospy
import setup_path 
import airsim

import sys
import time
import argparse
import pprint

import numpy  as np

from geometry_msgs.msg import Point32
from sensor_msgs.msg import LaserScan,PointCloud2
from sensor_msgs.msg import PointField
from std_msgs.msg import Header
from sensor_msgs import point_cloud2


def pub_pointcloud(points):
	pc = PointCloud2()
	pc.header.stamp = rospy.Time.now()
	pc.header.frame_id = 'velodyne'
	pc.height = 1
	pc.width = len(points)

	# pc.fields = [
	# 		PointField('x', 0, PointField.FLOAT32, 1),
	# 		PointField('y', 4, PointField.FLOAT32, 1),
	# 		PointField('z', 8, PointField.FLOAT32, 1),
	# 		PointField('intensity', 16, PointField.FLOAT32, 1),
	# 		PointField('ring', 20, PointField.UINT16, 1)]

	# pc.fields = [
	# 		PointField('x', 0, PointField.FLOAT32, 1),
	# 		PointField('y', 4, PointField.FLOAT32, 1),
	# 		PointField('z', 8, PointField.FLOAT32, 1),
	# 		PointField('intensity', 12, PointField.FLOAT32, 1),
	# 		PointField('ring', 16, PointField.UINT16, 1),
	# 		PointField('time', 18, PointField.FLOAT32, 1)]

	pc.fields = [
			PointField('x', 0, PointField.FLOAT32, 1),
			PointField('y', 4, PointField.FLOAT32, 1),
			PointField('z', 8, PointField.FLOAT32, 1),
			PointField('intensity', 12, PointField.FLOAT32, 1)]

	pc.is_bigendian = False
	# pc.point_step = 18
	# pc.point_step = 22
	pc.point_step = 16
	pc.row_step = pc.point_step * points.shape[0]
	pc.is_dense = True

	pc.data = np.asarray(points, np.float32).tostring()

	return pc

def main():

	# connect the simulator
	client = airsim.MultirotorClient()
	client.confirmConnection()
	client.enableApiControl(True)
	client.armDisarm(True)


	pointcloud_pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=10)
	rate = rospy.Rate(1.0)

	while not rospy.is_shutdown():

		# get the lidar data
		lidarData = client.getLidarData()
		#print('lidar',lidarData)

		if len(lidarData.point_cloud) >3:

			points = np.array(lidarData.point_cloud,dtype=np.dtype('f4'))
			points = np.reshape(points,(int(points.shape[0]/3),3))
			num_temp = np.shape(points)[0]
			# print(num_temp)
			numpy_temp = np.zeros(num_temp)
			numpy_temp1 = np.ones(num_temp)
			# print(numpy_temp)
			# points = np.c_[points,numpy_temp1,numpy_temp,numpy_temp]
			points = np.c_[points,numpy_temp1]

			#print('points:',points)
			pc = pub_pointcloud(points)

			pointcloud_pub.publish(pc)
			rate.sleep()
		else:
			print("tNo points received from Lidar data")

if __name__ == "__main__":
	rospy.init_node('UGV_lidar')
	main()

点云类型为XYZI型，其他类型可稍作修改

二、Airsim雷达参数设置

参考：https://blog.csdn.net/xiaowei1234565/article/details/106797708/

我的settings.json文件如下：

{
  "ClockSpeed": 1,
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "SubWindows": [
    {"WindowID": 0, "CameraName": "0", "ImageType": 3, "VehicleName": "", "Visible": false},
    {"WindowID": 1, "CameraName": "0", "ImageType": 5, "VehicleName": "", "Visible": false},
    {"WindowID": 2, "CameraName": "0", "ImageType": 0, "VehicleName": "Drone1", "Visible": true}
  ],
  "Vehicles": {
    "my_UGV": {
      "VehicleType": "SimpleFlight",
      "Sensors": {
        "imu_1": {
          "SensorType": 2,
          "Enabled": true,
          "AngularRandomWalk": 0.3,
          "GyroBiasStabilityTau": 500,
          "GyroBiasStability": 4.6,
          "VelocityRandomWalk": 0.24,
          "AccelBiasStabilityTau": 800,
          "AccelBiasStability": 36
        },
        "lidar_1": {
          "SensorType": 6,
          "Range": 100,
          "Enabled": true,
          "NumberOfChannels": 16,
          "PointsPerSecond": 288000,
          "RotationsPerSecond": 10,
          "VerticalFOVUpper": 15,
          "VerticalFOVLower": -15,
          "HorizontalFOVStart": -180,
          "HorizontalFOVEnd": 180,
          "X": 2, "Y": 0, "Z": -1,
          "DrawDebugPoints": true,
          "Pitch":0, "Roll": 0, "Yaw": 0,
          "DataFrame": "SensorLocalFrame"
        }
      },
      "Cameras": {
        "camera_1": {
          "CaptureSettings": [
            {
              "ImageType": 0,
              "Width": 672,
              "Height": 376,
              "FOV_Degrees": 90,
              "TargetGamma": 1.5
            }
          ],
          "X": 2, "Y":0, "Z":-1,
          "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0
        }
      },
      "X": 0, "Y": 0.0, "Z": -2,
      "Pitch":0, "Roll": 0, "Yaw": 0
    }
  }
}

需要注意：

• 雷达参数要和lego-loam里的对应
• DataFrame要设置为SensorLocalFrame。默认是汽车坐标系，会发生严重漂移

三、实验结果

仿真环境如下：

lego-loam运行结果：