本文主要介绍，利用Airsim中自己的场景跑通Lego-Loam；并且画出Lego-Loam的里程计轨迹，与真值轨迹(目前只能完成基本的建图功能，画轨迹部分待完善，坐标系搞得有些蒙)

1、设置setting文件

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "ViewMode": "SpringArmChase",
  "ClockSpeed": 0.1,
  "Vehicles": {
    "drone_1": {
      "VehicleType": "SimpleFlight",
      "DefaultVehicleState": "Armed",
      "EnableCollisionPassthrogh": false,
      "EnableCollisions": true,
      "AllowAPIAlways": true,
      "Sensors": {
        "Imu" : {
          "SensorType": 2,
          "Enabled": true,
          "AngularRandomWalk": 0.3,
          "GyroBiasStabilityTau": 500,
          "GyroBiasStability": 4.6,
          "VelocityRandomWalk": 0.24,
          "AccelBiasStabilityTau": 800,
          "AccelBiasStability": 36
        },        
        "LidarCustom": {
          "SensorType": 6,
          "Range": 50,
          "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"
        }
      }
    }
  }
}

2、配置环境变量

在bashrc文件中添加(换成自己的)

添加Airsim的ros功能包的环境变量这样就不用每次运行都souce一次

source ~/work/UE4.25-master/AirSim/ros/devel/setup.bash

3、利用Airsim录制rosbag

因为Airsim非常的消耗资源，同时开Airsim和Lego-Loam会比较卡，所以可以先录制好rosbag再跑Lego-Loam

打开UE4项目

进入到UE4-master文件夹

./Engine/Binaries/Linux/UE4Editor

选择打开项目

之后点击运行出现无人机

如果不想看到这些雷达的绿线可以在setting文件将 "DrawDebugPoints": 设为false

运行airsim的ros节点

必须运行airsim的ros节点才能将airsim仿真器与ros联通。从而使用ros的一些命令

注意：当前应在airsim的ros工作空间下

roslaunch airsim_ros_pkgs airsim_node.launch

此时可以查看airsim仿真器的节点

首先查看话题用如下命令

rostopic list

以上是airsim仿真器发布的话题

将点云转换成Lego-Loam需要的类型

Airsim的雷达发布的点云类型XYZIRT，但是airsim雷达发布的激光雷达话题为XYZ所以需要编写成学将其转换为XYZITRT

参考：

Airsim通过ros发布激光雷达数据+Lego-loam仿真测试(1)_鲮鲤猫的博客-CSDN博客_airsim ros

#!/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()

最好将这个程序放在如下路径

AirSim/PythonClient/multirotor

或者

AirSim/PythonClient/car

 PythonClient下其他路径或许也可以我没有试过

其中发布话题名称为velodyne_points和Lego-Loam输入话题名一致

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

让无人机飞起来

'''
Author: your name
Date: 2021-12-28 19:02:32
LastEditTime: 2022-02-14 19:51:37
LastEditors: Please set LastEditors
Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
FilePath: /AirSim/PythonClient/multirotor/zheng.py
'''
import setup_path
import airsim
import time
import numpy as np
import os
import tempfile
import pprint
import cv2
 
 
client = airsim.MultirotorClient()  # connect to the AirSim simulator
client.enableApiControl(True)       # 获取控制权
client.armDisarm(True)              # 解锁
client.takeoffAsync().join()        # 第一阶段：起飞
 
client.moveToZAsync(-0.5, 1).join()   # 第二阶段：第一个参数是高度，第二个参数是向上飞的速度,
 
 # 飞正方形
#client.moveByVelocityZAsync(1, 0, -1, 18).join()     # 第一个参数y方向速度(初始前方后方）向前为正，向后为负。第二个参数x方向速度(初始时向右为正，向左为负)，最后一个参数是飞行时间
client.moveByVelocityZAsync(0, -2, 0.5, 20).join()    
# client.moveByVelocityZAsync(-1, 0, -2, 8).join()   
# client.moveByVelocityZAsync(0, -1, -2, 8).join()   
 
 # 悬停 2 秒钟
client.hoverAsync().join()          # 第四阶段：悬停6秒钟
time.sleep(6)
 
client.landAsync().join()           # 第五阶段：降落
client.armDisarm(False)             # 上锁
client.enableApiControl(False)      # 释放控制权

参考

AirSim系列(6)-四旋翼飞正方形(速度控制) - 知乎

开始录制

1、打开项目

./Engine/Binaries/Linux/UE4Editor

2、运行airsim的ros节点(再打开一个终端)

roslaunch airsim_ros_pkgs airsim_node.launch

3、转换点云类型(再打开一个终端)

cd到点云转换代码路径我的路径是

AirSim/PythonClient/car

运行

python3 con_velodyne.py

再次查看发布的话题可以看到多了

/velodyne_points

4、让飞机飞起来(再打开一个终端)

cd到控制飞机飞行代码所在路径运行

python3 zheng.py //我的控制飞机飞行代码名字

5、录制(再打开一个终端)

rosbag record -a//录制所有话题

录制完成后会在当前目录生成一个以年月日十分妙命名的bag文件

查看

rosbag info xxx.bag

使用以上命令查看自己录制包的信息

 可以看到话题类型分别为

airsim_ros_pkgs

nav_msgs/Odometry

nav_msgs/Odometry Documentation

tf2_msgs/TFMessage

ROS学习笔记(十三） TF介绍（一）_bai君的博客-CSDN博客_tf数据是什么

 sensor_msgs

表示一些传感器的信息

 4、修改Lego-Loam

修改launch文件

<param name="/use_sim_time" value="true" />
true代表使用bag
false表示不用bag

画出运动轨迹

    <node pkg="path_3d" type="path_3d"    name="path_3d"    output="screen"/>
    <node pkg="path_3d" type="path_3d_drone"    name="path_3d_drone"    output="screen"/>

轨迹代码

注意：

这一个画轨迹的功能包是自己添加的需要事先编译；代码如下

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <iostream>
#include <fstream>
 
nav_msgs::Path  path;
ros::Publisher  path_pub;
 
void pathCallback(const nav_msgs::Odometry::ConstPtr& odom_3d)
{
   
    geometry_msgs::PoseStamped position_3d;
    position_3d.pose.position.x = odom_3d->pose.pose.position.x; 
    position_3d.pose.position.y = odom_3d->pose.pose.position.y; 
    position_3d.pose.position.z = odom_3d->pose.pose.position.z;
    position_3d.pose.orientation = odom_3d->pose.pose.orientation;
 
 
    position_3d.header.stamp = odom_3d->header.stamp;
    position_3d.header.frame_id = "map";
 
    path.poses.push_back(position_3d);
    path.header.stamp = position_3d.header.stamp;
    path.header.frame_id = "map";
    path_pub.publish(path);
    //std::cout<<"map" << odom_3d -> header.stamp << ' ' << position_3d.pose.position.x << ' ' << position_3d.pose.position.y  << ' ' << position_3d.pose.position.z << std::endl;
    //std::cout <<"my"<< odom_3d -> header.stamp << ' ' << odom_3d->pose.pose.position.x << ' ' << odom_3d->pose.pose.position.y << ' ' << odom_3d->pose.pose.position.z << std::endl;
    
    std::ofstream foutC("/home/du/work/path_ws/src/odom.txt", std::ios::app);
   // std::cout<<"chenggongbaocun"<<std::endl;
    foutC.setf(std::ios::fixed, std::ios::floatfield);
   // 保存结果的精度
    foutC.precision(5);
    foutC << odom_3d->header.stamp << " "
         << odom_3d->pose.pose.position.x << " "
         << odom_3d->pose.pose.position.z << " "
         <<odom_3d->pose.pose.position.y << " "
         << odom_3d->pose.pose.orientation.x << " "
         << odom_3d->pose.pose.orientation.y << " "
         << odom_3d->pose.pose.orientation.z << " "
         << odom_3d->pose.pose.orientation.w << std::endl;
         foutC.close();
}
 
int main (int argc, char **argv)
{
    ros::init (argc, argv, "showpath");
    ros::NodeHandle ph;
 
    path_pub = ph.advertise<nav_msgs::Path>("odom3d_path", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 10, pathCallback);  //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改
    //ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/airsim_node/drone_1/odom_local_ned", 10, pathCallback);  //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改
    ros::Rate loop_rate(1000);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

解释

geometry_msgs::PoseStamped position_3d;
    position_3d.pose.position.x = odom_3d->pose.pose.position.x; 
    position_3d.pose.position.y = odom_3d->pose.pose.position.y; 
    position_3d.pose.position.z = odom_3d->pose.pose.position.z;
    position_3d.pose.orientation = odom_3d->pose.pose.orientation;
position_3d.header.stamp = odom_3d->header.stamp;
    position_3d.header.frame_id = "map";

path_pub = ph.advertise<nav_msgs::Path>("odom3d_path", 10, true);
    ros::Subscriber odomSub = ph.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 10, pathCallback);  //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改

 /laser_odom_to_init是里程计话题名（Lego-Loam发出的话题）

Cmake

cmake_minimum_required(VERSION 2.8.3)
project(path_3d)
 
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
 
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
message_generation
)
 
## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   geometry_msgs   std_msgs
 )
 
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES path_3d
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
  DEPENDS system_lib
)
 
include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)
 
add_executable(path_3d src/path_3d.cpp) #${PROJECT_NAME}_node
target_link_libraries(path_3d ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node
add_dependencies(path_3d beginner_tutorials_generate_messages_cpp) #path_3d_node
 
add_executable(path_3d_drone src/path_drone.cpp) #${PROJECT_NAME}_node
target_link_libraries(path_3d_drone ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node
add_dependencies(path_3d_drone beginner_tutorials_generate_messages_cpp) #path_3d_node

 其中

project(path_3d)

所以在launch文件中node pkg要写成path3d

修改utility文件

extern const string pointCloudTopic = "/velodyne_points";
extern const string imuTopic = "/Imu/data";
输入的节点名称，在airsim仿真器进行点云变换时已经将输出点云话题名称改成了/velodyne_points所以此处无需修改，如果你的点云话题名称不是这个，需要修改
imu信息可以没有，当bag没有/Imu/data这个话题名时就没有imu的输入，效果也可以
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
不是velodyne激光雷达，所以没有ring将其设置为false

运行

还是先将环境变量写到bashrc中

打开一个终端cd到Lego-Loam工作空间运行

roslaunch roslaunch lego_loam run.launch

再打开一个终端播放bag

rosbag play yuantu.bag --clock

如果想快一点儿播放可以用

rosbag play -r 5  yuantu.bag --clock

代表以5倍的速度播放

查看一些信息

查看节点信息

 圆圈是节点，方块是话题

 查看TF-tree

rosrun rqt_tf_tree rqt_tf_tree

结果

参考

ros学习（一）——rosbag使用_xiaohu的博客-CSDN博客_rosbag

Airsim通过ros发布激光雷达数据+Lego-loam仿真测试(1)_鲮鲤猫的博客-CSDN博客_airsim ros

AirSim系列(6)-四旋翼飞正方形(速度控制) - 知乎

nav_msgs/Odometry Documentation

ROS学习笔记(十三） TF介绍（一）_bai君的博客-CSDN博客_tf数据是什么