Mavros在线控制 (Python)

注意：本文适用于所有Kerloud无人机产品

python 语言使用mavros 进行offboard控制的方式与C++ 案例相同。该方法是利用 mavros 包中的标准主题或服务来检索车辆信息或发送命令。 工作区位于 ~/src/uav_workspace/pythonuav_ros，所有必要的软件依赖在出厂阶段已配置。

1. 环境设置

本教程的依赖项为：

• mavros：具有不同ROS版本的标准mavros包，安装指令：sudo apt install ros-<ROS_VERSION>-mavros

• mavlink：具有不同ROS版本的标准mavlink包，安装指令：sudo apt install ros-<ROS_VERSION>-mavlink

用户可以参考工作区中的 README 文件了解详情。

出于兼容性考虑，我们还在 github 中维护了定制版的 mavros 和 mavlink 包，用户可参考：

• mavros（dev_kerlouduav 分支）：https://github.com/cloudkernel-tech/mavros

• mavlink（dev_kerlouduav 分支）：https://github.com/cloudkernel-tech/mavlink-gdp-releas

要使用这些包，用户必须创建一个新的 catkin 工作空间，且每次启动这些自定义节点时都需要通过 source devel/setup.bash 获取环境变量。

2. 代码说明

工作区包含两个程序文件：commander.py 和 px4_mavros_run.py。command.py（指令接口）为应用开发提供了一个友好接口，px4_mavros_run.py（控制接口）封装了与mavros通信的底层消息。

2.1 指令接口

指令接口为用户编程提供友好的 API，如 move()、turn()、hover()、land() 等命令。接口示例如下：

    if __name__ == "__main__":

        # the mission performs a square waypoint flight with position target defined in FLU frame
        con = Commander()
        time.sleep(2)
        con.move(5, 0, 0)
        time.sleep(5)
        con.move(0, -5, 0)
        time.sleep(5)
        con.move(-5, 0, 0)
        time.sleep(5)
        con.move(0, 5, 0)
        time.sleep(5)
        con.land()
        time.sleep(5)

commander类首先将被实例化为一个对象，然后休眠 2 秒。默认情况下，程序将要求机体前往 ENU 坐标系下定义的几个航路点。请依据情况调整休眠时间，以确保机体能够到达所需位置。 基于例程，用户可以轻松添加更多的无人机动作，如悬停、转弯等。

2.2 控制接口

控制接口实现与mavros网关的底层通信，大多数情况下无需更改。入口函数为Px4Controller类中的start()，函数在执行px4_mavros_run.py时会被调用。

start() 函数中，首先会初始化 ros 节点，然后程序将等待 IMU 航向数据就绪，以便为机体控制提供初始目标位姿。该程序将首先命令机体起飞到所需高度（默认为 3.2m）。

    rospy.init_node("offboard_node")
    for i in range(10):
        if self.current_heading is not None:
            break
        else:
            print("Waiting for initialization.")
            time.sleep(0.5)
    self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
                                                 self.local_pose.pose.position.y,
                                                 self.local_pose.pose.position.z + self.takeoff_height,
                                                 self.current_heading)

然后程序将解锁机体并自动切换到offboard模式：

    for i in range(100):
        self.local_target_pub.publish(self.cur_target_pose)
        self.arm_state = self.arm()
        self.offboard_state = self.offboard()
        time.sleep(0.05)

此后，程序将进入一个while循环来响应来自指令接口的不同命令，包括位置设定点、转弯、悬停等。

    while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):

        self.local_target_pub.publish(self.cur_target_pose)

        if (self.state is "LAND") and (self.local_pose.pose.position.z < -1.0):

            if(self.disarm()):

                self.state = "DISARMED"


                time.sleep(0.05)

3. 如何运行SITL仿真

强烈建议用户在实际飞行前进行 SITL 仿真测试，SITL环境的搭建过程可以参考之前的C++版offboard控制教程。

假定固件库位于 ~/src/Firmware 目录下，基于gazebo的SITL仿真可通过如下方式启动：

    # launch a new terminal
    cd ~/src/Firmware
    make px4_sitl_default gazebo

    # launch a new terminal
    roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

    # launch a new terminal
    cd ~/src/uav_workspace/pythonuav_ros
    python3 px4_mavros_run.py

    # launch a new terminal
    cd ~/src/uav_workspace/pythonuav_ros
    python3 commander.py

4. 实际飞行

建议用户严格按照以下流程完成测试：

• 用绑带固定好电池，确保在飞行过程中不会掉落。

• 确保所有开关都处于初始位置，且油门拉至最低。

• 上电开机。

• 等待 GPS 锁星：通常 2-3 分钟后我们会听到提示音，并且 pixhawk 中的主 LED 会变绿。

• 确保安全员处于待命状态，且机体周围没有人。等待几分钟让机载计算机启动，通过本地 wifi 网络远程登录，确认 ~/src/uav_workspace/pythonuav_ros/commander.py 中定义的航点任务， 然后运行如下命令启动offboard控制模块：

    # launch a terminal (users can source customized mavros & mavlink workspace alternatively)
    roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"
  
    # launch a new terminal
    cd ~/src/uav_workspace/pythonuav_ros
    python3 px4_mavros_run.py
  
    # launch a new terminal
    cd ~/src/uav_workspace/pythonuav_ros
    python3 commander.py

• 机体将起飞并自主执行所定义的任务。如果发生任何意外，请务必切换到 POSITION 模式。