前言
本文所使用的仿真软件是开源的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)
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明rotors无人机仿真！
