ROS学习记录（五）树莓派外部控制 Pixhawk（一站到底）

---

序言

---

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

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

---

一、系统环境

---

软硬件名称	版本	备注
树莓派	Raspberry Pi 3B	无
树莓派操作系统	Ubuntu 16.04 MATE	无
树莓派安装库	ROS (Kinetic) 和 MAVROS	Moledic 似乎只支持Ubuntu18.04 及以上
飞控	Pixhawk 2.4.8	无
飞控固件版本	PX4 v1.8.２ (stable)	无
笔记本操作系统	Ubuntu 16.04	如果不做源码编译与仿真，Windows也可
笔记本关键软件	QGroundControl v3.5.6	用于下载固件
笔记本关键软件	putty	用于远程登录树莓派
其他硬件	USB转TTL	无

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

---

二、飞控板设置

---

2.1 下载固件 v1.10.1 (stable)

打开QGC --> 点击设置（齿轮图标） --> 点击固件 --> 插上飞控 --> 点击确定。

注：可能需要重新拔插飞控版。操作如下图：
升级完毕，重新拔插使得固件有效。

2.2 使用外部控制

搜索参数 RC_MAP_OFFB_SW，设置为 Channel1

同样的方式，设置 SYS_COMPANION 为 57600

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

---

三、树莓派程序

---

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

3.1 创建程序包

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

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

cd pi_offboard_px4/src
touch offboard_pi_node.cpp
vim offboard_pi_node.cpp

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

  /**
 * @file offb_node.cpp
 * @brief offboard example node, written with mavros version 0.14.2, px4 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, "offboard_pi_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);
　　　
　　 ROS_INFO("Pass 1");    //原例程没有，加入方便debug
　　 
    // 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();
    
	ROS_INFO("Pass ２");    //原例程没有，加入方便debug
	
    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/pi_offboard_px4/
vim CMakeLists.txt

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

add_executable(offboard_pi_node src/px4_pi_node.cpp)
target_link_libraries(offboard_pi_node 
	${catkin_LIBRARIES}
)

3.3 编译

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 pi_offboad_px4 offboard_pi_node

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

rostopic list
rostopic echo /mavros/altitude

查看当地海拔高度．

运行效果如下图

---

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

---

来源：CSDN

作者：大强强小强强

链接：https://blog.csdn.net/weixin_41869763/article/details/104258058