使用Python语言进行在线控制（单机）

注意：此教程适用于DASA室内配置

使用python编程语言控制单台机器可以作为入门用户的起点。DASA 室内环境中Kerloud机器（无人机、无人飞车和无人车）的在线控制是通过利用位于 ~/DASA_space/catkinws_dasa 的本地化工作区中提供的内部 Mavros API 实现的。请注意，此处Mavros 包是指为DASA 系统设计的专用版本，以支持扩展的机器操作，这里不能使用 ROS 社区中的标准版本。

1.文件目录

demo程序位于~/DASA_space/offboard_python_dasa，文件目录如下：

scripts/：方便程序运行的脚本，例如将整个目录复制到远程DASA agents；
commander.py：指令接口，实现机器人操作的各种行为，如位置移动、解锁机体等；
px4_mavros_run.py：控制接口，实现在 DASA 本地化工作区中与 Mavros 网关的底层通信；
uav_demo.py：Kerloud 无人机操作的演示程序；
flyingrover_demo.py：Kerloud 无人飞车操作的演示程序；
rover_demo.py：Kerloud 无人车操作的演示程序。

2.环境要求

推荐环境为Ubuntu 18.04，ROS melodic和python 3.6.9。

本教程的依赖项为：

mavros (dev_kerlouddasa branch): https://github.com/cloudkernel-tech/mavros
mavlink (dev_kerlouddasa branch): https://github.com/cloudkernel-tech/mavlink-gdp-release

python模块要求：

python3 -m pip install rospkg pyquaternion

DASA 本地化工作区默认位于 ~/DASA_space/catkinws_dasa 中，用户只需运行 setup.sh 即可安装其他先决条件。所有必要的软件依赖在出厂阶段均已配置。

3.代码说明

3.1 Commander接口

Commander接口实现机器人操作的各种行为，如位置移动、解锁机体等。背后的原因是不同的DASA机器在操作层面共享相似的行为接口，因此用户只需调用这些功能即可构建应用程序。接口示例如下：

初始化函数：

__init__() 函数初始化变量、订阅和发布。

def __init__(self, flag_dasa=False)

callback模块

这部分声明了 ros 订阅的回调函数。

# callback for local position infor
def local_pose_callback(self, msg):
    self.local_position = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]

def local_velocity_callback(self, msg):
    self.local_velocity = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z]

...

Behavior控制模块

Behavior控制函数用于位置控制、速度控制和命令请求，请注意里面的框架定义。这些函数支持 ENU（东-北-上）和 DASA 坐标系中的运动。

# move to a target position
def move_position(self, x, y, z, BODY_FLU=False):
    self.position_target_pub.publish(self.set_pose(x, y, z, BODY_FLU))

def move_velocity(self, vx, vy, vz=0, BODY_FLU=False):
    self.velocity_target_pub.publish(self.set_velocity(vx, vy, vz, BODY_FLU))

# turn to a desired yaw angle
def turn(self, yaw_degree):
    self.yaw_target_pub.publish(yaw_degree)

...

支持功能模块

该模块包含一些用于姿态或速度构造和数学计算的支持函数。例如，以下函数构造航点路径的请求坐标。

# set waypoint pose
def set_pose(self, x=0, y=0, z=2, BODY_FLU = False):
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()

    # ROS uses ENU internally, so we will stick to this convention
    if BODY_FLU:
        pose.header.frame_id = 'base_link'
    else:
        pose.header.frame_id = 'map'

    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.position.z = z

    return pose

3.2 Control控制接口

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

在 start() 函数中，ros 节点将首先通过对本地化工作空间的服务调用等待 DASA 坐标初始化，在下一步操作之前，需要设置好自动驾驶数据。

# waiting for dasa frame initialization
if self.flag_dasa_frame:
    print("Waiting for DASA frame initialization")
    self.getDasaFrameInfo()

print("Waiting for autopilot data...")

while (self.current_heading is None or self.local_pose is None):
    time.sleep(1.0)

self.cur_pos_target = self.construct_position_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...")

    while True:
        self.local_target_pub.publish(self.cur_pos_target)

        if not self.arm_state:
            self.arm()

        if not self.offboard_state:
            self.offboard()

        if self.arm_state and self.offboard_state:
            print("vehicle is armed and set in offboard mode")
            break

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

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

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

    # publications
    if self.cmd_mode == MC_POSITION_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_pos_target)
    elif self.cmd_mode == MC_VELOCITY_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_vel_target)
    elif self.cmd_mode == ROVER_POSITION_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_pos_target)
    elif self.cmd_mode == ROVER_VELOCITY_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_vel_target)
    elif self.cmd_mode == ROVER_ACTUATOR_CONTROL_ROVER_COMMAND_MODE:
        self.actuator_control_target_pub.publish(self.cur_actuator_control_target)

    if self.vehicle_type == "flyingrover":
        self.flyingrover_mode_pub.publish(self.current_fr_mode)

    self.landedstate_pub.publish(String(self.landed_state))

...

4.虚拟仿真测试

操作流程

强烈建议用户在实际飞行前进行虚拟仿真测试。虚拟仿真环境在 ~/DASA_space/virtual_simulation 目录中，适用于所有 Kerloud系列机型。请注意，仿真测试只能在个人计算机上执行。

# Attention: users have to start STIL simulation for Kerloud vehicles in virtual environment first
# uav case:
cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_uav
bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud300
# flying rover case:
# cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_flyingrover
# bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud_fr_scorpion
# rover case:
# cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_rover
# bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud_rover

# terminal 1
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# launch the mavros node for simulation
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# terminal 2: start the commander interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# start demo programs for a specific vehicle type
python3 uav_demo.py --flag_dasa False
# python3 flyingrover_demo.py --flag_dasa False
# python3 rover_demo.py --flag_dasa False

# terminal 3: start the control interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle uav
# python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle flyingrover
# python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle rover

或者直接使用便捷脚本：

bash scripts/start_uav_sitldemo.sh              # uav case
bash scripts/start_flyingrover_sitl_demo.sh     # flyingrover case
bash scripts/start_rover_sitl_demo.sh           # rover case

5.DASA环境中的实际飞行

操作流程

建议用户严格按照以下流程完成测试。请记住，安全第一，每次飞行都应有一名安全员在场。

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

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

步骤三：打开网络名称为 DASA_swarm 的 DASA wifi 路由器，以及实验设置中的 UWB 设备。用户根据用户手册正确配置DASA室内定位系统。

步骤四：确保安全员处于待命状态，并且机器周围没有人。打开电源，等待几分钟让车载电脑启动。 DASA 系统提供了一个随时可用的 DASA_swarm 网络，所有 Kerloud 系列机器都可以在启动时自动连接。对于每台机器，其主机名配置了一个唯一的 ID。

要检查ID为x的机器是否连接到本地网络，只需在主计算机中ping其ip或主机名即可。

ping <vehicle ip address: 192.168.0. 100+x> or ping ubuntu<x>

从目标 ip 地址返回响应，便能远程操作机器。

步骤五：必须将 python 工作区同步到远程机器，只需运行 scripts/ 文件夹中的脚本：

bash scripts/sync_python_demo_remote.sh

步骤六：使用 ssh 命令登录远程机器：

ssh ubuntu@<vehicle ip address: 192.168.0. 100+x>  or ssh ubuntu@ubuntu<x>

然后执行以下命令。请注意，在运行 python 演示代码之前，必须启动本地化节点。

# terminal 1: start DASA localization nodes
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/catkinws_dasa && source devel/setup.bash
# launch nodes for dasa localization
bash scripts/single_case/<run_script>.sh

# terminal 2: start the control interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
python3 px4_mavros_run.py --sim False --flag_dasa True --vehicle <vehicle_type>

# terminal 3: start the commander interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# start demo programs for a specific vehicle type
python3 uav_demo.py --flag_dasa True
# python3 flyingrover_demo.py --flag_dasa True
# python3 rover_demo.py --flag_dasa True

# then users can arm and set offboard mode with a transmitter to start the mission

或者直接使用便捷脚本，然后使用遥控器进行操作：

bash scripts/run_uav_demo.sh             # uav case
bash scripts/run_flyingrover_demo.sh     # flyingrover case
bash scripts/run_rover_demo.sh           # rover case

6.演示视频

DASA Offboard 控制的虚拟仿真: