多旋翼无人机仿真 rotors_simulator：用键盘控制无人机飞行
- 前言
- 书接上文
- 接口测试
- 键盘指令发布
- 指令转换与发布
- 修改 rotors_simulator 的控制接口节点
- 测试
前言
RotorS 是一个MAV gazebo 仿真系统。
提供了几种多旋翼仿真模型，例如
- AscTec Hummingbird
- AscTec Pelican
- AscTec Firefly
但是仿真系统不限于使用这几种模型
AscTec 是 德国Ascending Technologies公司的缩写。
是很早的无人机了，实物张下面这个样子：
仿真系统中包含很多种仿真传感器，都可以安装在无人机上，例如：
- IMU
- 里程计
- 视觉惯导相机
功能包中包含了几种控制器，包含位置控制，游戏手柄控制灯
github的地址为：https://github.com/ethz-asl/rotors_simulator
书接上文
在上一篇博客中 ： https://www.guyuehome.com/40362 分析了 rotors_simulator 自带的一个控制接口
需要输入 roll pitch yawrate thrust 的指令即可控制无人机的飞行。
但是在那篇文章也说了，它自带的键盘控制的节点功能启动不成功，导致无法在gazebo里控制无人机飞。
本篇博客将基于上篇分析的控制接口，再写一个键盘的指令发布功能，对应到接口指令的转换，来控制无人机先飞起来。
接口测试
RollPitchYawrateThrustControllerNode.cpp 中
送到控制器的 控制指令变量类型是这个
mav_msgs::EigenRollPitchYawrateThrust
这变量类型的定义是这样的
所以推力是 三维 的
给z轴一个推力，那么可以这样写
在里程计的回调函数里
mav_msgs::EigenRollPitchYawrateThrust roll_pitch_yawrate_thrust;
// 先自己固定一个控制量
roll_pitch_yawrate_thrust.roll = 0.1;
roll_pitch_yawrate_thrust.thrust.z() = 15;
// 送入控制器中
roll_pitch_yawrate_thrust_controller_.SetRollPitchYawrateThrust(roll_pitch_yawrate_thrust);
相当于 给一直给 z轴一个15的推力 期望的横滚角是0.1 弧度
可看到飞机横着飞了起来
ok，接口测通了，那么可自己写个键盘的指令发送器了。
键盘指令发布
键盘的指令发布有很多的现成功能包
我使用的最多的就是 teleop_twist_keyboard 这个功能包。
运行的时候终端会出现下面的界面：
从输出的信息上可以看出，其功能是发布Twist的速度控制指令。
其源码如下：
#!/usr/bin/env python
from __future__ import print_function
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""
moveBindings = {
'i':(1,0,0,0),
'o':(1,0,0,-1),
'j':(0,0,0,1),
'l':(0,0,0,-1),
'u':(1,0,0,1),
',':(-1,0,0,0),
'.':(-1,0,0,1),
'm':(-1,0,0,-1),
'O':(1,-1,0,0),
'I':(1,0,0,0),
'J':(0,1,0,0),
'L':(0,-1,0,0),
'U':(1,1,0,0),
'<':(-1,0,0,0),
'>':(-1,-1,0,0),
'M':(-1,1,0,0),
't':(0,0,1,0),
'b':(0,0,-1,0),
}
speedBindings={
'q':(1.1,1.1),
'z':(.9,.9),
'w':(1.1,1),
'x':(.9,1),
'e':(1,1.1),
'c':(1,.9),
}
def getKey():
tty.setraw(sys.stdin.fileno())
select.select([sys.stdin], [], [], 0)
key = sys.stdin.read(1)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
def vels(speed,turn):
return "currently:\tspeed %s\tturn %s " % (speed,turn)
if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
rospy.init_node('teleop_twist_keyboard')
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print(vels(speed,turn))
while(1):
key = getKey()
if key in moveBindings.keys():
x = moveBindings[key][0]
y = moveBindings[key][1]
z = moveBindings[key][2]
th = moveBindings[key][3]
elif key in speedBindings.keys():
speed = speed * speedBindings[key][0]
turn = turn * speedBindings[key][1]
print(vels(speed,turn))
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
x = 0
y = 0
z = 0
th = 0
if (key == '\x03'):
break
twist = Twist()
twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
pub.publish(twist)
except Exception as e:
print(e)
finally:
twist = Twist()
twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
pub.publish(twist)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
通过查看源码可知，该功能包最终通过扫描键盘按键，
最终发布 topic 名称为 cmd_vel 的消息
消息的格式为：geometry_msgs::Twist
指令转换与发布
由于键盘指令发布功能包 发布的命令和rotors_simulator我想用的控制接口的命令不一致
所以需要写一个节点来进行控制指令的转换
下面给出指令转换节点的 核心思想和重要代码
ros::Subscriber keyboard_cmd_vel_sub_;
声明订阅句柄
keyboard_cmd_vel_sub_ = nh.subscribe("/cmd_vel",1,&PidPositionControllerNode::KeyboardCmdVelCallback,this);
订阅句柄赋值 /cmd_vel 就是键盘指令发布节点发布的topic的名称
然后实现其回调函数
void KeyboardCmdVelCallback(const geometry_msgs::TwistConstPtr& cmdvel_msg)
{
// 取出指令
geometry_msgs::Twist cmd_vel = *cmdvel_msg;
roll =gain_roll* cmd_vel.linear.x ;
pitch = gain_pitch* cmd_vel.linear.y ;
thrust = gain_thrust* cmd_vel.linear.z ;
yawrate = gain_yawrate* cmd_vel.angular.z ;
}
将指令转换成 roll pitch thrust yawrate的指令
ros::Publisher Control_RollPitchYawrateThrust_pub_;
声明转换后指令的发布句柄
Control_RollPitchYawrateThrust_pub_ = nh.advertise<mav_msgs::RollPitchYawrateThrust>("control_keyboard_cmd",1);
赋值句柄，将发布的消息名称定义为：control_keyboard_cmd
void PubControlMsg()
{
// 控制量
mav_msgs::RollPitchYawrateThrust roll_pitch_yawrate_thrust;
roll_pitch_yawrate_thrust.thrust.z= thrust_z;
roll_pitch_yawrate_thrust.roll = roll_;
roll_pitch_yawrate_thrust.pitch = pitch_;
roll_pitch_yawrate_thrust.yaw_rate = yawrate_;
Control_RollPitchYawrateThrust_pub_.publish(roll_pitch_yawrate_thrust);
}
最后写一个发布指令msg的函数，完成对接口指令的发布
修改 rotors_simulator 的控制接口节点
下面需要做的就是修改rotors_simulator的控制接口节点
也就是roll_pitch_yawrate_thrust_controller_node.cpp
ros::Subscriber Control_RollPitchYawrateThrust_sub_;
声明订阅指令的句柄
Control_RollPitchYawrateThrust_sub_ = nh.subscribe("/control_pid_pos", 1,
&RollPitchYawrateThrustControllerNode::ControlCallback, this);
赋值订阅句柄，消息名称要是 control_keyboard_cmd 和上面的能对应上
// 控制指令回调函数
void RollPitchYawrateThrustControllerNode::ControlCallback(
const mav_msgs::RollPitchYawrateThrustConstPtr& roll_pitch_yawrate_thrust_reference_msg) {
// 转成eigen的格式
mav_msgs::EigenRollPitchYawrateThrust roll_pitch_yawrate_thrust;
mav_msgs::eigenRollPitchYawrateThrustFromMsg(*roll_pitch_yawrate_thrust_reference_msg, &roll_pitch_yawrate_thrust);
// 送入控制器中
roll_pitch_yawrate_thrust_controller_.SetRollPitchYawrateThrust(roll_pitch_yawrate_thrust);
}
在回调函数中，将接收的指令，完成对控制器的输送。
测试
下面则可以打开 gazebo 和控制节点、键盘发布节点和指令转换节点，来进行测试了。
无人机可以按照期望的指令飞行。
但是无人机非常的不好控制，比如上下需要不断的改变推力，使得无人机高度保持。
也需要不断的改变姿态，使得无人机不至于飘的太远。
这是因为控制接口实现的仅是姿态控制，垂直上，没有实现闭环控制。
下一节将基于roll pitch yawrate thrust 控制接口，并订阅无人机里程计数据，实现pid闭环控制。使得无人机飞行更加稳定。
