多旋翼无人机仿真 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闭环控制。使得无人机飞行更加稳定。