ROS综合应用（一）树莓派外部控制 Pixhawk（一站到底）

---

序言

---

上一次 说到学习 ROS，主要是因为在做多无人机协同控制项目。听说可以使用 ROS 来实现树莓派控制 Pixhawk 无人机，这样可以不用修改飞控源码，而且可以用树莓派做图像处理。于是开始了一场浩浩荡荡的西行，可奈何刚翻过几座山，又越过几条河，妖魔鬼怪怎么这么多。不过一条总结是多去官网，博客鱼龙混杂，而且很多时候版本不合，不可使用。

折腾了好久好久，终于完成了环境搭建，后面也测试成功，因此想记录一下。
注：初学外部控制时，请勿挂上螺旋桨，否则很危险！！！

---

一、系统环境

---

软硬件名称	版本	备注
树莓派	Raspberry Pi 3B	无
树莓派操作系统	Ubuntu 16.04 MATE	无
树莓派和笔记本	ROS (Melodic) 和 MAVROS	Moledic 似乎只支持Ubuntu18.04 及以上
树莓派和笔记本	python 要默认为 python2	不然安装 ROS 会有问题
飞控	Pixhawk 2.4.8	无
飞控固件版本	PX4 v1.8.２ (stable)	要自己编译下载（不要怕，此文一一道来）
笔记本操作系统	Ubuntu 18.04	编译固件，相传 Windows编译源码很慢
笔记本关键软件	QGroundControl v3.5.6	用于设置飞控 TELEM波特率
笔记本关键软件	putty	用于远程登录树莓派
其他硬件	USB转TTL	和无人机TELEM 2 通信时需要

假设你已经完成以上软硬件安装，注意QGC 使用的最新PX4 固件为 v1.10.1 ，找不到下面的　SYS_COMPANION　参数，不知为何，就是不行，程序卡在自己编写的第一个　while() 循环里。至于如何下载自己想要的版本，下次专门说．

---

二、飞控板设置

---

飞控板的主设置主要实现下载固件 v1.8.2 (stable)

2.1 登出再重新登入用户

sudo usermod -a -G dialout $USER

然后Ubuntu18.04 就是右上角注销，然后再登陆。

2.2 克隆飞控源码

 git clone https://github.com/PX4/Firmware.git		# 下载源码
 git tag											# 查看发行版本
 git checkout v1.8.2  								# 选择需要的版本

2.3 搭建编译环境

安装必须环境，有问题请参考 PX4 Developer Guide

首先下载 ubuntu_sim_common_deps.sh，直接点进去复制里面内容或者 wget 下载皆可尝试，有时会连接不上，然后运行，这里面相当于把许多 shell 指令集合在一起，一次性安装完成。

# 下载 ubuntu_sim_common_deps.sh
wget https://raw.githubusercontent.com/PX4/Devguide/v1.9.0/build_scripts/ubuntu_sim_common_deps.sh
bash ubuntu_sim_common_deps.sh

2.4 编译与上传

Pixhawk 对应的固件在这里。我的是 Pixhawk2.4.8，也就是Pixhawk2，因此选择 px4fmu-v3_default 。

make px4fmu-v3_default								# 编译（第一次还会下载很多模块，耐心等待）
make px4fmu-v3_default upload						# 编译上传

此时连上飞控，就可以下载，正常的话会得到如下界面：

2.5 使能外部控制

如果传感器这些没有校正，请先校正，遥控器和飞行模式可以不用管。然后继续往下。

搜索参数 RC_MAP_OFFB_SW，设置为 Channel1

同样的方式，设置 SYS_COMPANION 为 57600

然后重新拔插，使设置有效。

---

三、树莓派设置（完全可以先用笔记本测试）

---

在树莓派上，需要新增一个 ROS 程序包，然后这个包里添加自己读取的程序。这一步骤与 ROS学习笔记（二）——文件系统、创建并编译程序包 的 2.2 创建ROS程序包 的步骤基本一致，如下：

3.1 配置环境

安装MAVROS，此处使用二进制安装（比官网推荐的源安装方便很多）

sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras ros-kinetic-control-toolbox

安装地理数据集

sudo /opt/ros/kinetic/lib/mavros/install_geographiclib_datasets.sh		# 可能会有点慢，耐心等待

3.2 创建程序包

cd ~/catkin_ws/src
catkin_create_pkg offb geometry_msgs mavros roscpp
#程序包名称 pi_offboard_px4，依赖的包 geometry_msgs, mavros 和 roscpp

3.3 添加 .cpp 文件及必要修改

cd offb/src
touch offb_node.cpp
vim offb_node.cpp

添加以下来自 PX4官方教程的MAVROS控制例程：

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

注：如果手写以上内容，很可能出错，编译产生Invocking "make -j4 -l4" failed，请根据错误多次排查。

接下来修改 CMakeLists.txt 以便编译。

cd ~/catkin_ws/src/offb/
vim CMakeLists.txt

取消注释/修改文本，得到如下两句：

add_executable(offb src/offb_node.cpp)
target_link_libraries(offb
	${catkin_LIBRARIES}
)

3.4 编译

catkin_make 				#编译
catkin_make install
source devel/setup.bash     #配置catkin 工作空间

---

四、连接测试（完全可以先用笔记本测试）

---

4.1 连接

使用USB转TTL 连接树莓派 与Pixhawk 的TELEM 2 端口 ，TELEM 2 输入电压为5V， USB转TTL使用5V的。

飞控板 TELEM2 字是正的，那么从左到右六个接口分别为：

5V TX RX 无用 无用 GND

实物连接图如下，从Pixhawk到树莓派的线的真实连接为：红-紫，白-灰，黑-绿，黄-蓝。

4.2 测试

这个过程需要树莓派打开几个终端，可以多登录几个 putty 实现。
在一个终端中运行 roscore ，启动 ROS。

roscore

在另一个终端中运行

ls /dev/tty*        # 下面两句只运行一句即可（根据有 ttyUSB0 还是 ttyACM0）
roslaunch mavros px4.launch fcu_url:=/dev/ttyUSB0:57600  #有ttyUSB0
roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0:57600  #有ttyACM0

再在另一个终端运行

rosrun offb offb_node

此时还可以再另一个终端运行

rostopic list
rostopic echo /mavros/altitude

查看当地海拔高度。当然，如果在 offboard_pi_node.cpp 里添加这些话题，也直接能够读取处理打印。

运行效果如下图

---

参考连接：PX4 无人机MAVROS 外部控制教程

---