Offboard Control 例程 (ROS python)

本例程介绍在陆地车、多旋翼模式下，如何使用ROS python API操作飞车。

Caution

📌 例程代码仅适于项目研究，不可直接用于产品。如果您在测试中发现错误，可随时在我们的官方资源库中创建pull请求。

1. 环境设置

本教程推荐的环境为Ubuntu18.04系统下ROS melodic，或者我们发行的树莓派model 3b+的ubuntu 16.04和ROS kinetic。

相关依赖：

mavros (dev_flyingrover 分支): https://github.com/cloudkernel-tech/mavros
mavlink (dev_flyingrover 分支): https://github.com/cloudkernel-tech/mavlink-gdp-release
offboard control node (dev_flyingrover 分支): https://github.com/cloudkernel-tech/offboard_demo_python
python module requirements: python3 -m pip install rospkg pyquaternion

用户可通过如下指令更新到最新版本：

cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavros
git pull origin
git checkout dev_flyingrover

cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavlink
git pull origin
git checkout dev_flyingrover

cd ~/src/flyingrover_workspace/offboard_demo_python
git pull origin
git checkout dev_flyingrover

由于Kerloud产品软件在持续更新，如果用户在上述目录没有找到对应代码，可以通过以下指令创建工作区。

mkdir ~/src/flyingrover_workspace
mkdir ~/src/flyingrover_workspace/catkinws_offboard 
cd ~/src/flyingrover_workspace/catkinws_offboard
catkin init
cd src

git clone --recursive https://github.com/cloudkernel-tech/mavros
cd mavros && git checkout dev_flyingrover

cd ..
git clone --recursive https://github.com/cloudkernel-tech/mavlink-gdp-release mavlink
cd mavlink && git checkout dev_flyingrover

cd ~/src/flyingrover_workspace/
git clone https://github.com/cloudkernel-tech/offboard_demo_python
cd offboard_demo_python && git checkout dev_flyingrover

关于mavros、mavlink工作区的编译，请参考前节内容（Offboard控制例程（ROS C++））。

2. 代码讲解

python offboard control的工作区位于~/src/flyingrover_workspace/offboard_demo_python，包含commander.py、px4_mavros_run.py两个程序文件。commander.py（命令接口）提供对用户友好的应用开发接口，而px4_mavros_run.py（控制接口）对底层与mavros间的消息通信进行了封装。

2.1 Commander接口

命令接口为用户编程提供友好的API，如move(), turn(), transit_to_mc()等。此程序主要目的是演示在陆地车、多旋翼模式下实现航路点任务。我们按如下顺序对代码进行讲解：命令类首先被例化为对象，程序将等待控制接口就绪，然后就可以发布命令。注意，飞车仅在成功解锁并处于offboard模式下才能执行指定的任务。

con = Commander()
time.sleep(2)

# waiting for core to be ready
while not con.flag_core_ready:
    print("waiting for the vehicle to be armed and in offboard")
    time.sleep(1)

开始时，飞车处于陆地车模式。默认情况下，下面的代码将请求飞车前往ENU坐标系下的几个航路点。请确保调整点间运动所需的休眠时间，以确保飞车能够到达指定的位置。

con.move(5, 0, 0)
time.sleep(5)
con.move(5, 5, 0)
time.sleep(5)
con.move(0, 5, 0)
time.sleep(5)
con.move(0, 0, 0)
time.sleep(5)

陆地车模式下任务完成后，程序将请求将飞车切换为多旋翼模式。

# request to transit to multicopter
while not con.flyingrover_mode == "MC":
    print("request to transit to multicopter mode")
    con.transit_to_mc()
    time.sleep(2)

然后，飞车将执行为多旋翼模式设置的航路点任务。为安全考虑，任务结束后将锁定飞车。

# multicopter mission round
con.move(5, 0, 5)
time.sleep(5)
con.move(5, 5, 5)
time.sleep(5)
con.move(0, 5, 5)
time.sleep(5)
con.move(0, 0, 5)
time.sleep(5)
con.land()
time.sleep(5)

#disarm for safety at the end
con.disarm()

2.2 控制接口

控制接口实现底层与mavros网关间的通信，多数情况下此处内容无需改动。入口函数是Px4Controller类中的start()，该函数将在执行px4_mavros_run.py时被调用。 start()函数会先初始化ros节点，然后等待IMU航向数据就绪，以便为飞车命令构建初始目标姿态。

rospy.init_node("offboard_node")
rate = rospy.Rate(30)  # 30Hz

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.current_heading)

仿真模式下（暂未支持），程序将解锁飞车并设置为offboard模式。

'''arm and set offboard automatically in simulation mode'''
if flag_simulation_mode:
    print("setting arm and offboard in simulation mode...")

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

        if self.arm_state and self.offboard_state:
            break

        time.sleep(0.05)

最后，程序将进入一个while循环来响应指令接口发出的不同指令，如位置设置、模式切换、上锁/解锁等。

while not rospy.is_shutdown():
     # publications
     self.local_target_pub.publish(self.cur_target_pose)
     self.flyingrover_mode_pub.publish(self.current_fr_mode)
     self.landedstate_pub.publish(String(self.landed_state))

     # publish flag to indicate the vehicle is armed and in offboard
     if self.arm_state and self.offboard_state:
         self.core_ready_pub.publish(True)
     else:
         self.core_ready_pub.publish(False)
     ......

3. 如何进行实测

建议用户严格按如下流程来进行测试：

将电池及线固定好，确保在飞行中不会发生脱落。
确保所有开关都处于初始位置，且油门通道已拉到最低。
电池上电。
等待GPS锁定：通常2-3分钟后我们可听到提示音，同时飞控上的LED会变为绿色。

确保飞手已就位，且飞车周围没有人。等待几分钟，机载电脑启动后可通过本地Wi-Fi网络进行远程登录，确认 ~/src/offboard_flyingrover_python/commander.py目录下的航路点任务，然后通过如下指令启动offboard控制单元：

  # launch a terminal
  cd ~/src/flyingrover_workspace/catkinws_offboard
  source devel/setup.bash
  roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

  # launch a new terminal
  cd ~/src/flyingrover_workspace/offboard_demo_python
  python3 px4_mavros_run.py

  # launch a new terminal
  cd ~/src/flyingrover_workspace/offboard_demo_python
  python3 commander.py

拉低油门摇杆，同时右推偏航摇杆进行机器解锁，将听到一个长音提示。
使用特定的摇杆（例如通道7）切换到offboard模式，飞车将自动启动航路点任务。恭喜飞行成功！