前言

本文所使用的仿真软件是开源的Rotors包,仅供学习使用!然后这里说明下,只是个人爱好仿真下无人机玩玩,对官方给的无人机控制代码理解的也不是很深刻,大家可以自己摸索摸索自己开发,这里会提供大家下载安装教程以及我已经尝试过的玩法教程,后续可能会继续更新。
项目官方github地址:rotors项目地址
我的github地址:我的项目地址

安装rotors

首先创建工作区间:

mkdir -p rotors_ws/src
cd rotors_ws/src
catkin_init_workspace
git clone https://github.com/xmy0916/rotors

编译代码:

cd ..
catkin_make

编译成功如下图:

 


 

这里如果大家报错这种格式的少了什么msg(这里用ackermann_msgs为例):

error: ackermann_msgs/AckermannDrive.h: No such file or directory

 

这种题目是有固定答案的同学们:

sudo apt-get install ros-kinetic-ackermann-msgs

 

把少了的消息改成短横杠然后加上ros版本就行了,学会变通嘛!
使能环境变量;

echo "source ~/rotors_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

加载飞机

我这里选择使用的是六轴的飞机,带有三个摄像头。
启动街道仿真场景:

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

这里编辑器有bug……截个小图吧:

 


 

这里还有许多别的场景,大家可以自己修改launch文件:

sudo gedit ~/rotors_ws/src/rotors_simulator/rotors_gazebo/launch/mav_hovering_example_with_vi_sensor.launch

 

修改下图中第四行的参数outdoor:

 



所有选项如下图:

编写开飞机脚本

在官方的代码中已经有了发布位置导航的代码,不过是cpp代码而且没有键盘控制,今天主要教大家怎么修改成py的键盘控制代码,原代码如下:

/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <fstream>
#include <iostream>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

int main(int argc, char** argv) {
  ros::init(argc, argv, "waypoint_publisher");
  ros::NodeHandle nh;
  ros::Publisher trajectory_pub =
      nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

  ROS_INFO("Started waypoint_publisher.");

  ros::V_string args;
  ros::removeROSArgs(argc, argv, args);

  double delay;

  if (args.size() == 5) {
    delay = 1.0;
  } else if (args.size() == 6) {
    delay = std::stof(args.at(5));
  } else {
    ROS_ERROR("Usage: waypoint_publisher <x> <y> <z> <yaw_deg> [<delay>]\n");
    return -1;
  }

  const float DEG_2_RAD = M_PI / 180.0;

  trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
  trajectory_msg.header.stamp = ros::Time::now();

  Eigen::Vector3d desired_position(std::stof(args.at(1)), std::stof(args.at(2)),
                                   std::stof(args.at(3)));

  double desired_yaw = std::stof(args.at(4)) * DEG_2_RAD;

  mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position,
      desired_yaw, &trajectory_msg);

  // Wait for some time to create the ros publisher.
  ros::Duration(delay).sleep();

  while (trajectory_pub.getNumSubscribers() == 0 && ros::ok()) {
    ROS_INFO("There is no subscriber available, trying again in 1 second.");
    ros::Duration(1.0).sleep();
  }

  ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].",
           nh.getNamespace().c_str(),
           desired_position.x(),
           desired_position.y(),
           desired_position.z());

  trajectory_pub.publish(trajectory_msg);

  ros::spinOnce();
  ros::shutdown();

  return 0;
}

首先看源码发布的消息是什么话题:

nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

 

根据这两句代码,发现话题是COMMAND_TRAJECTORY,所以使用ros工具rostopic查看详情。
查看全部话题:

rostopic list

 

查看话题的消息类型

rostopic info /firefly/command/trajectory

 

查看消息详情

rosmsg info trajectory_msgs/MultiDOFJointTrajectory

 

 

或者在API网站上查询消息详情
网站地址:ros的api网站

 

 

根据上面消息类型分析
trajectory_msgs/MultiDOFJointTrajectory消息下主要封装了两个类型的消息:

  • string[] joint_names:列表类型
  • trajectory_msgs/MultiDOFJointTrajectoryPoint[] points:列表类型

trajectory_msgs/MultiDOFJointTrajectoryPoint消息主要封装了三种消息类型:

  • geometry_msgs/Transform[] transforms:列表类型
  • geometry_msgs/Twist[] velocities:列表类型
  • geometry_msgs/Twist[] accelerations:列表类型

编写按键控制代码

首先先把turtlebot的按键控制代码抄过来,修改这一段核心代码:

            twist = Twist()

            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
            twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0

            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
            twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

            pub.publish(twist)

 

按照上述分析修改代码:

            # 创建MultiDOFJointTrajectory消息
            trajectory_msg = MultiDOFJointTrajectory()
            trajectory_msg.joint_names.append("base_link")
            trajectory_points = MultiDOFJointTrajectoryPoint()

            #创建transform消息
            tran = Transform()
            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0)) 
            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0)) 
            pos_x += control_linear_vel
            pos_y += control_angular_vel
            tran.translation.z = 1
            tran.translation.x = pos_x
            tran.translation.y = pos_y

            # 创建velocities消息
            twist1 = Twist()
            # 创建accelerations消息
            twist2 = Twist()

            # 合成trajectory_points消息
            trajectory_points.transforms.append(tran)
            trajectory_points.accelerations.append(twist1)
            trajectory_points.velocities.append(twist2)

            # 合成trajectory_msg消息
            trajectory_msg.points.append(trajectory_points)

            pub.publish(trajectory_msg)

 

我相信代码已经注释的很详细了,完整的代码在工程的下面目录中

~/rotors_ws/src/rotors_simulator/rotors_gazebo/scripts/teleop_key.py

开飞机

启动仿真场景:

roslaunch rotors_gazebo mav_hovering_example_with_vi_sensor.launch

 

运行脚本:

cd ~/rotors_ws/src/rotors_simulator/rotors_gazebo/scripts
python teleop_key.py

 

按键wasdx可以控制前后左右急停运动,暂时没加旋转

 

代码附件

键盘控制代码:

# -*- coding: UTF-8 -*-
#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Transform
from trajectory_msgs.msg import MultiDOFJointTrajectory
from trajectory_msgs.msg import MultiDOFJointTrajectoryPoint

import sys, select, os
import mav_msgs.msg as mav
if os.name == 'nt':
  import msvcrt
else:
  import tty, termios

FETCH_MAX_LIN_VEL = 1
FETCH_MAX_ANG_VEL = 1

LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.01

msg = """
Control Your uav!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (FETCH : ~ 0.22)
a/d : increase/decrease angular velocity (FETCH : ~ 2.84)

space key, s : force stop

CTRL-C to quit
"""

e = """
Communications Failed
"""

def getKey():
    if os.name == 'nt':
      return msvcrt.getch()

    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def vels(target_linear_vel, target_angular_vel):
    return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def constrain(input, low, high):
    if input < low:
      input = low
    elif input > high:
      input = high
    else:
      input = input

    return input

def checkLinearLimitVelocity(vel):
    vel = constrain(vel, -FETCH_MAX_LIN_VEL, FETCH_MAX_LIN_VEL)
    return vel

def checkAngularLimitVelocity(vel):
    vel = constrain(vel, -FETCH_MAX_ANG_VEL, FETCH_MAX_ANG_VEL)
    return vel

if __name__=="__main__":
    if os.name != 'nt':
        settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('rotors_teleop')
    pub = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size=10)

    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0

    pos_x = 0
    pos_y = 0
    pos_z = 1

    try:
        print(msg)
        while(1):
            key = getKey()
            if key == 'w' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'x' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'a' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'd' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == ' ' or key == 's' :
                target_linear_vel   = 0.0
                control_linear_vel  = 0.0
                target_angular_vel  = 0.0
                control_angular_vel = 0.0
                print(vels(target_linear_vel, target_angular_vel))
            else:
                if (key == '\x03'):
                    break

            if status == 20 :
                print(msg)
                status = 0

            # 创建MultiDOFJointTrajectory消息
            trajectory_msg = MultiDOFJointTrajectory()
            trajectory_msg.joint_names.append("base_link")
            trajectory_points = MultiDOFJointTrajectoryPoint()

            #创建transform消息
            tran = Transform()
            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0)) # 限速
            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0)) # 限速
            pos_x += control_linear_vel
            pos_y += control_angular_vel
            tran.translation.z = 1
            tran.translation.x = pos_x
            tran.translation.y = pos_y

            # 创建velocities消息
            twist1 = Twist()
            # 创建accelerations消息
            twist2 = Twist()

            # 合成trajectory_points消息
            trajectory_points.transforms.append(tran)
            trajectory_points.accelerations.append(twist1)
            trajectory_points.velocities.append(twist2)

            # 合成trajectory_msg消息
            trajectory_msg.points.append(trajectory_points)

            pub.publish(trajectory_msg)

    except:
        print(e)

    finally:
        # control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
        # twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
        #
        # control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
        # twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

        trajectory_msg = MultiDOFJointTrajectory()
        trajectory_msg.joint_names.append("base_link")

        trajectory_points = MultiDOFJointTrajectoryPoint()

        tran = Transform()

        twist = Twist()

        twist2 = Twist()
        trajectory_points.transforms.append(tran)
        trajectory_points.accelerations.append(twist)
        trajectory_points.velocities.append(twist2)
        # control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
        # twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0
        #
        # control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
        # twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

        trajectory_msg.points.append(trajectory_points)
        pub.publish(trajectory_msg)

    if os.name != 'nt':
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)