多旋翼无人机仿真 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 pythonfrom __future__ import print_functionimport roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospyfrom geometry_msgs.msg import Twistimport sys, select, termios, ttymsg = """
Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:u    i    oj    k    lm    ,    .For Holonomic mode (strafing), hold down the shift key:
---------------------------U    I    OJ    K    LM    <    >t : up (+z)
b : down (-z)anything else : stopq/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 keydef 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 = 0y = 0z = 0th = 0status = 0try: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) % 15else:x = 0y = 0z = 0th = 0if (key == '\x03'):breaktwist = 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*turnpub.publish(twist)except Exception as e:print(e)finally:twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.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("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闭环控制。使得无人机飞行更加稳定。