.. _tutorial_offboard_python_zh:

Offboard Control 例程(ROS python)
=============================================

本教程提供了如何在各种命令模式下使用 ROS python API 操作 Autocar 的示例。

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

环境设置
------------------

推荐环境为使用 Ubuntu 16.04 的 ROS kinetic，尽管本教程也可以在其他 ROS 版本中运行。

现成可用的ROS工作区位于~/src/rover_workspace/offboard_demo_python目录 下。依赖项如下：

* mavros（dev_rover 分支）：[https ://github.com/cloudkernel-tech/mavros](https://github.com/cloudkernel-tech/mavros)

* mavlink（dev_rover 分支）：[https ://github.com/cloudkernel-tech/mavlink-gdp-release](https://github.com/cloudkernel-tech/mavlink-gdp-release)

* offboard 控制节点（dev_rover 分支）：[https ://github.com/cloudkernel-tech/offboard_demo_python](https://github.com/cloudkernel-tech/offboard_demo_python)

* python模块要求：

::

        python3 -m pip install rospkg pyquaternion numpy

要更新到最新版本，用户可以执行以下命令：

::

        cd ~/src/rover_workspace/catkinws_offboard/mavros
        git pull origin
        git checkout dev_rover

        cd ~/src/rover_workspace/catkinws_offboard/mavlink
        git pull origin
        git checkout dev_rover

        cd ~/src/rover_workspace/offboard_demo_python
        git pull origin
        git checkout dev_rover


若要搭建mavros和mavlink的workspace，请参考之前的C++ offboard control教程。python 代码不需要编译。

2.代码说明
------------------

python offboard control的工作区位于~/src/rover_workspace/offboard_demo_python目录下，由两个程序文件组成：commander.py和px4_mavros_run.py。command.py（Commander Interface）为应用开发提供了一个友好接口，px4_mavros_run.py（Control Interface）封装了与mavros的底层消息通信。建议用户通读README文件。

2.1 命令接口
^^^^^^^^^^^^^^^

命令接口为用户编程提供了友好的 API，例如 move_position()、move_velocity() 等命令。该程序的主入口提供了在各种命令模式下操作车体的示例。指令模式与之前的 C++ offboard控制教程相同。我们下面依次解释主代码：

命令类首先会被实例化为一个对象，程序将等待控制接口准备好，以便稍后发布命令。请注意，车辆只有在解锁并处于offboard模式时才能执行所需的任务。

::

        # initialize the commanding interface
        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坐标系下定义的几个航路点。请确保调整车辆行驶的休眠时间，使其能够到达所需位置。

::

        # position guidance cmd
        print("Commander interface: start position control mode")
        con.move_position(5, 0, 0)
        time.sleep(5)
        con.move_position(5, 5, 0)
        time.sleep(5)


然后车辆将进入速度引导模式，并默认跟踪在 ENU 框架中定义的所需速度。请注意，API 还支持 FLU（前左上）帧中的速度跟踪。

::

        # velocity guidance cmd
        print("Commander interface: start velocity control mode")
        con.move_velocity(0.3, 0.3, 0.3, False)
        time.sleep(5)


之后，车辆进入姿态引导模式和直接执行器控制模式。在姿态引导模式下，用户可以设置偏航值及所需的油门值。在直接执行器控制模式下，用户可以为转向舵机和主电机设置所需的执行器输入。这些 API 可用于高级运动操作，如轨迹跟踪、入侵演练等。

::

        # attitude guidance cmd
        print("Commander interface: start attitude control mode")
        print("set forward driving")
        con.set_forward_driving()
        con.set_yaw_and_throttle(3.14/2, 0.3)
        time.sleep(5)

        # direct actuator control cmd
        print("Commander interface: start actuator control mode")
        con.set_steering_servo_and_throttle(0.5, 0.3)
        time.sleep(5)
        con.set_backward_driving()
        print("set backward driving")
        con.set_steering_servo_and_throttle(0.5, 0.3)


最后，车辆将被锁定。

::

        #disarm for safety at the end
        print("Commander interface: disarm vehicle, mission completed")
        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("Control interface: waiting for initialization.")
                time.sleep(0.5)

        self.cur_pos_target = self.construct_position_target(self.local_pose.pose.position.x,
                                                     self.local_pose.pose.position.y)


在模拟模式下，程序将为车辆解锁并设置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_pos_target)
                self.arm_state = self.arm()
                self.offboard_state = self.offboard()

                if self.arm_state and self.offboard_state:
                    break

                time.sleep(0.05)
        else:
            print("Control interface: starting in real test...")


最后，程序会进入一个while循环来响应来自Commander界面的不同命令，包括位置设定、速度设定、前进/后退行驶设置命令、解锁或锁定请求等。

::

        '''
        main ROS thread
        '''
        while not rospy.is_shutdown():

            # publications

            if self.cmd_mode == POSITION_ROVER_COMMAND_MODE:
                self.local_target_pub.publish(self.cur_pos_target)
            elif self.cmd_mode == VELOCITY_ROVER_COMMAND_MODE:
                self.local_target_pub.publish(self.cur_vel_target)
            elif self.cmd_mode == ATTITUDE_ROVER_COMMAND_MODE:
                self.attitude_target_pub.publish(self.cur_attitude_target)
                self.thrust_target_pub.publish(self.cur_thrust_target)
            elif self.cmd_mode == ACTUATOR_CONTROL_ROVER_COMMAND_MODE:
                self.actuator_control_target_pub.publish(self.cur_actuator_control_target)

            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.如何进行实测
^^^^^^^^^^^^^^^^

现在是时候进行真机实测了。由于 Autocar 主要用于室内应用，因此在实践此offboard控制教程之前应先启动定位模块。例如，我们必须为 Autocar Laser 启动激光定位算法。然后，用户就可以按照如下命令来避免意外行为：

* 确保遥控器的所有开关都处于初始位置，并且油门拉至低位。
* 电池上电开机。
* 通过局域网远程登录车载电脑，确认在命令接口中定义的任务。
* 首先启动定位模块（如laser slam），并通过rostopic 命令验证位置输出。

::

        # terminal 1
        # source the mavros package in the c++ offboard ros workspace
        cd ~/src/rover_workspace/offboard_demo_python
        source ../catkinws_offboard/devel/setup.bash
        # launch the mavros node for real tests
        roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

        # terminal 2
        cd ~/src/rover_workspace/offboard_demo_python
        source ../catkinws_offboard/devel/setup.bash
        python3 px4_mavros_run.py

        # terminal 3
        cd ~/src/rover_workspace/offboard_demo_python
        source ../catkinws_offboard/devel/setup.bash
        python3 commander.py

* 通过拉低油门并右推偏航杆实现车体解锁，然后我们会听到机器发出长长的哔哔声。
* 使用指定的摇杆（例如通道 7）切换到 OFFBOARD 模式，然后机器将自动开始任务。 运行成功 !