初始Airsim(五)之PX4、Mavros控制

前言

mavros包允许在运行ROS的计算机、支持MAVLink的飞控板以及支持MAVLink的地面站之间通讯。MAVROS可以用来与任何支持MAVLink的飞控板通讯。

基于mavros和mavlink协议,实现对airsim的控制

说明

针对上一节中mavros的配置,在安装./install_geographiclib_datasets.sh(二进制安装)或者./src/mavros/mavros/scripts/install_geographiclib_datasets.sh(源码安装)时,最好加上proxychains4,即翻墙安装,因为在重新测试的过程中,初始没有加proxychains4,启动launch文件时,一直报GeographicLib exception相关的错误。

错误提示信息

1
2
3
4
5
6
7
8
9
10
11
12
13
[FATAL] [1455208235.573568497]: UAS: GeographicLib exception: File not readable /usr/share/GeographicLib/geoids/egm96-5.pgm | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!

================================================================================REQUIRED process [mavros-2] has died!
process has died [pid 2396, exit code -6, cmd /home/ubuntu/catkin_ws_wyz/devel/lib/mavros/mavros_node __name:=mavros __log:=/home/ubuntu/.ros/log/c5fc1ef8-d0dc-11e5-89a3-00044b65bbf5/mavros-2.log].
log file: /home/ubuntu/.ros/log/c5fc1ef8-d0dc-11e5-89a3-00044b65bbf5/mavros-2*.log
Initiating shutdown!
================================================================================
[mavros-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

并且,参考网上的方法都没有作用:参考1参考2,其实这两种方法说的都是同一种,即加权限重新安装。

最后在安装时,添加了proxychains4后,则没有报错,上一节也已经更新。

一、mavros控制

前提:已经按照上一节的配置,安装了mavros、mavlink及PX4的编译等,且settings.json也配置为PX4

1.1 启动环境

(1)编译PX4

编译,等待UDP的连接

1
2
cd PX4/Firmware
make posix_sitl_ekf2 none_iris

(2)运行UE4Editor,启动UE4环境,加载airsim

UE4Editor文件在~/UnrealEngine/Engine/Binaries/Linux/目录下

双击运行UE4Editor即可

(3)启动px4.launch文件

1
2
3
cd ~/catkin_ws
source devel/setup.bash
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

1.2 mavros控制

基于UE4Editor启动,并加载的Rolling环境

(1)起飞降落的命令控制

根据GPS的坐标进行起飞降落控制,目前还不能通过命令实现drone的飞行

1
2
3
rosservice call /mavros/cmd/arming true
rosservice call /mavros/cmd/takeoff -- 0 0 47.6420 -122.1404 126.1
rosservice call /mavros/cmd/land -- 0 0 47.6420 -122.1404 123

1.2.1 代码组成

主要是参考darknight-007/docking的代码,并对他/她的代码进行测试,发现存在一点问题,于是在testVelocityControl.py中添加了自己的代码,目前飞行没有问题,主要基于速度控制实现了一个长方形的飞行控制,其轨迹图见3.4

直接查看所有代码

主要包含三部分,PID.py、VelocityController.py、testVelocityControl.py。其中PID.py和VelocityController.py和参考的代码完全一致,没有做改动,主要是修改testVelocityControl.py等

1.2.2 testVelocityControl.py结构

目前起飞时通过设置的GPS坐标来起飞的,即不论drone在哪,都会在设置的GPS点处进行起飞,虽然降落同样也是设置的和起飞的GPS坐标差不多,但是它是当前位置降落

关于起飞和降落这一部分还存在一些问题

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
#轨迹点(4个轨迹点,起飞后依次到达,实现长方形的飞行控制)
navigate = [
[0,7,3],
[30,7,3],
[30,0,3],
[0,0,3]
]
navigate_count = 0 #用于记录当前的目标点

class QuadController:
#给target目标赋值
target = Pose()
target.position.x = navigate[navigate_count][0]
target.position.y = navigate[navigate_count][1]
target.position.z = navigate[navigate_count][2]

def __init__(self):
self.desire_v = 1.5 #设定期望速度
while not rospy.is_shutdown():
#起飞后,要将mode修改为OFFBOARD才能控制飞行
set_mode = rospy.ServiceProxy('/mavros/set_mode',SetMode)
mode = set_mode(custom_mode = 'OFFBOARD')
print('change mode to OFFBOARD:',count)
count += 1
#且必须要发布topic才行,不发布修改mode不起作用(还不太明白是为啥)
self.des_vel = vController.update(self.cur_pose)
vel_pub.publish(self.des_vel)
rate.sleep()

if self.isReadyToFly:
x1,y1,z1 = self.cur_pose.pose.position.x,self.cur_pose.pose.position.y,self.cur_pose.pose.position.z

if((x1 - self.target.position.x) **2 +(y1 - self.target.position.y)**2 +(z1 - self.target.position.z) **2) < 0.5**2:#设定一个阈值,使得无人机当前到达目标区域附近即可更新目标点
#通过切换mode为LOITER实现悬停,否则会执行之前的速度指令
set_mode = rospy.ServiceProxy('/mavros/set_mode',SetMode)
mode = set_mode(custom_mode = 'LOITER')
rate.sleep()

#到达当前目标点后,更新目标点的下一个位置
global navigate_count
navigate_count += 1
if navigate_count < len(navigate):
self.target.position.x = navigate[navigate_count][0]
self.target.position.y = navigate[navigate_count][1]
self.target.position.z = navigate[navigate_count][2]
vController.setTarget(self.target)
print('cur_pose:',[x1,y1,z1])
#速度归一化为期望速度
self.des_vel = vController.update(self.cur_pose)
vx,vy = self.des_vel.twist.linear.x,self.des_vel.twist.linear.y
vx_ = 1.0 * vx / (math.sqrt(vx**2 + vy**2)) *self.desire_v
vy_ = 1.0 * vy / (math.sqrt(vx**2 + vy**2)) *self.desire_v

self.des_vel.twist.linear.x,self.des_vel.twist.linear.y = vx_,vy_

vel_pub.publish(self.des_vel)#发布速度指令
#到达所有轨迹点后,降落,同样要先悬停,进行速度清0
if navigate_count >= len(navigate):
set_mode = rospy.ServiceProxy('/mavros/set_mode',SetMode)
mode = set_mode(custom_mode = 'LOITER')
rate.sleep()
self.land()
break

def arming(self): #解锁
def takeoff(self):#起飞
def land(self):#降落

二、rviz安装及测试

安装rviz,是为了实时显示drone的飞行轨迹,轨迹也可以用QGC来显示,但是没有rviz好

2.1 rviz安装

安装rviz

1
sudo apt-get install ros-kinetic-rviz

环境检测及安装

在使用rosmake rviz命令的时候,提示我没有rosmake,因此在第2步,先安装了rosmake

1
2
3
rosdep install rviz
sudo apt-get install python-rosmake
rosmake rviz

2.2 运行rviz

在新终端运行roscore,启动rosmaster

1
roscore

在另一个新终端运行rviz

1
rosrun rviz rviz

三、rviz实时显示mavros控制飞行轨迹

3.1 启动UE4环境

参照1.1节

3.2 mavros控制代码里面发布相关topic

通过使用nav_msgs/Path来实现,其结构为

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

(1)添加相关代码(不完整)

主要是先获取drone的当前姿态数据,其数据结构见上,然后将该数据用nav_msgs/Path发布出去

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
from geometry_msgs.msg import Pose, PoseStamped
from nav_msgs.msg import Path

path = Path()
class QuadController:

def __init__(self):
pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.pos_cb)#通过该topic获取其相对位置坐标
self.path_pub = rospy.Publisher('/path',Path,queue_size=10)#发布topic


def pos_cb(self, msg):
# print msg
self.cur_pose = msg
global path
path.header = msg.header
pose = PoseStamped()
pose.header = msg.header
pose.pose = msg.pose
path.poses.append(pose)
self.path_pub.publish(path)

3.3 启动rviz

目前是通过在终端输入命令来启动rviz的,其实也可以写在launch文件里面

(1)命令启动rviz

​ 由于3.1节里启动了px4.launch文件,因此不需要再启动roscore

1
rosrun rviz rviz

(2)添加代码中发布的相关的topic

在打开的rviz界面中,点击Add,然后选择添加Path

则会看到,左边多出了Path的相关信息,然后在Topic的右侧写入刚才发布的topic(/path)

3.4 运行mavros控制代码

rviz显示

则在rviz里面能看到飞行轨迹了,我这里设置的mavros是飞行一个长方形的轨迹

3.5 说明

​ airsim里面也自带了一些rviz.launch文件,进行启动,我之前有测试过,启动launch之后,然后显示飞行轨迹时有报错,因此不采用这种方法,也没有去解决该错误,下图左边框出部分是报错信息,也没有成功显示飞行轨迹

​ 顺带介绍一下,运行airsim里自带的这些launch文件的步骤,github详情

(1)升级cmake(要求cmake3.10版本以上)

注意一定不要先卸载cmake,即执行**sudo apt-get autoremove cmake**命令,执行该命令后,会把ros的一些相关依赖和mavros给卸载掉,若执行了则又需要重装ros

​ 首先,去cmake官网手动下载cmake3.12.2,或者直接快捷跳到github下载地址,我这里采用命令下载

1
wget proxychains4 https://github.com/Kitware/CMake/releases/download/v3.12.2/cmake-3.12.2-Linux-x86_64.tar.gz

​ 解压:

1
tar zxvf cmake-3.12.2-Linux-x86_64.tar.gz

​ 创建软连接:

1
2
sudo mv cmake-3.12.2-Linux-x86_64 /opt/cmake-3.12.2
sudo ln -sf /opt/cmake-3.12.2/bin/* /usr/bin

​ 最后,查看cmake版本

1
2
3
4
5
6
cmake --version

输出:
cmake version 3.12.2

CMake suite maintained and supported by Kitware (kitware.com/cmake).

(2)编译(catkin build或catkin_make都可以)

1
2
cd Airsim/ros
catkin build

(3)运行

​ 只是为了显示rviz,运不运行airsim_node.launch都无所谓,但是添加了airsim_node.launch之后,rviz界面会多出一个东西,具体见下图

1
2
3
source devel/setup.bash
roslaunch airsim_ros_pkgs airsim_node.launch
roslaunch airsim_tutorial_pkgs rviz.launch

但是这个ros里面的airsim_node.launch,应该就发布了一些可以控制的命令topic,关于具体的控制,则没有去研究

四、rviz实时显示自定义ros控制飞行轨迹

这个关于自定义的ros控制,然后用rviz去显示器轨迹的时候,真的耗了我不少的功夫,原因在于网上的教程或说明更多的是教你如何写代码,并且用的是别人创建好的ros包,而且没有去获取机器人的位置。关于自己定义的ros包,没有什么说明。总之与我所面临的问题不太一样,后来经过测试,终于发现了其问题所在。

由于对ros和rviz不太熟,甚至最开始一度觉得自定义的ros,需要发布tf信息即可。而我的ros发布的消息并没有tf。直接说问题吧

原来的mavros控制下,其rviz相关代码如下,然后在rviz里面输入发布的topic就可以了

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
from geometry_msgs.msg import Pose, PoseStamped
from nav_msgs.msg import Path

path = Path()
class QuadController:

def __init__(self):
pos_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.pos_cb)#通过该topic获取其相对位置坐标
self.path_pub = rospy.Publisher('/path',Path,queue_size=10)#发布topic


def pos_cb(self, msg):
self.cur_pose = msg
global path
path.header = msg.header
pose = PoseStamped()
pose.header = msg.header
pose.pose = msg.pose
path.poses.append(pose)
self.path_pub.publish(path)

自定义ros的rviz显示,按照道理,和上面代码差不多,但有部分需要修改,最开始按照上面的代码运行后,在rviz里显示下面的错误(这个错误其实就是一直在提示map的Fixed Frame不对,不存在,在mavros是不需要这个地方的)

后来,将其代码进行修改,代码参考了玩转四旋翼无人机,但是并不需要进行tf广播,而且里面的tf广播好像还存在问题。修改后的代码如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
#!/usr/bin/env python
# -*-coding:utf-8 -*-

import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped

class path(object):
def __init__(self):
self.pose_sub = rospy.Subscriber('/drone1/airsimPose',PoseStamped,self.add_pose_path)
self.path_pub = rospy.Publisher('/drone1/path', Path, queue_size=10)
self.path = Path()

def add_pose_path(self,msg):
self.path.header = msg.header
self.path.header.frame_id = 'drone1' #这个frame_id很重要
pose = PoseStamped()
pose.header = msg.header
pose.pose = msg.pose
#下面的取相反值,是因为airsim的坐标和rviz的坐标不太一样,相反就刚好是其轨迹
#否则,airsim里向上值是负的,而rviz会显示的向下飞
pose.pose.position.z = -pose.pose.position.z
pose.pose.position.y = -pose.pose.position.y
self.path.poses.append(pose)
self.path_pub.publish(self.path)
print('path',[msg.pose.position])

if __name__=='__main__':
rospy.init_node('airsim_path')
mpath = path()
while not rospy.is_shutdown():
rospy.spin()

然后,在rviz里面,将Fixed Frame修改为自定义的frame_id,这样就不会报错。

最后,显示自定义的ros控制的飞行轨迹

总结

关于mavros,现在理解的还不太多,但对其控制逻辑也有了一个基本的了解,其逻辑如下

(1) 解锁:首先完成解锁动作

(2) 起飞:控制起飞(从特定的经纬度处起飞,暂时还没有对起飞进行优化)

(3) 模式切换:起飞后,若要执行飞行命令,则需要将mode切换为OFFBOARD(并且在切换的过程中,必须要发布命令,否则切换无效)。

(4) 给定轨迹点:通过距离差值给速度,并且设定期望速度,最后对速度进行归一化。

(5) 悬停:到达目标点后,需要进行悬停操作,否则,之前的速度还会存在影响,即对速度进行清零操作。Mavros的悬停操作为切换模式,将mode切换为LOITER即可实现悬停。

(6) 降落:降落前,最好也先悬停一下,在进行降落。同样,和起飞类似,也是给定经纬度进行降落,不过降落好像并不是在特定位置降落。

若要实现多个轨迹点的操作,则在到达目标点并悬停后,重新给定一个轨迹点即可。

参考链接

0%