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 模式，然后机器将自动开始任务。 运行成功 !