Airsim+Lego-Loam
本文主要介绍，利用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.bash3、利用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/carPythonClient下其他路径或许也可以我没有试过
其中发布话题名称为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/UE4Editor2、运行airsim的ros节点(再打开一个终端)
roslaunch airsim_ros_pkgs airsim_node.launch3、转换点云类型(再打开一个终端)
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文件
true代表使用bag
false表示不用bag画出运动轨迹
轨迹代码
注意：
这一个画轨迹的功能包是自己添加的需要事先编译；代码如下
#include
#include
#include
#include
#include
#include
#include
#include
#include
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"<header.stamp << " "
<< odom_3d->pose.pose.position.x << " "
<< odom_3d->pose.pose.position.z << " "
<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("odom3d_path", 10, true);
ros::Subscriber odomSub = ph.subscribe("/laser_odom_to_init", 10, pathCallback); //订阅里程计话题信息,其中"/odometry_3d"是自己发布的里程计话题名，别忘了修改
//ros::Subscriber odomSub = ph.subscribe("/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("odom3d_path", 10, true);
ros::Subscriber odomSub = ph.subscribe("/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数据是什么