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

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

本教程的offboard control demo是用C++语言重写的。虽然 ROS 环境同时支持 Python 和 C++ 语言，但我们强烈建议用户熟悉 C++，因为 C++ 在执行过程中比 Python 快得多，尤其是当用户需要和底层硬件打交道时。与 python 版本的主要区别在于 C++ demo的设计可以支持集群机器人，下面的章节将会介绍。

1.文件目录

该demo的工作区位于 ~/DASA_space/catkinws_offboard_dasa，它由两个包组成：

px4_dasa_bridge：该软件包提供了DASA 机器上px4 Kerloud 自动驾驶仪的接口。
offboard_demo：该软件包提供了一个非常简单的 DASA 机器航点飞行的demo。

2.环境要求

推荐环境为Ubuntu 18.04、ROS melodic和python 3.6.9。软件依赖与之前的python demo一样。定制的 Mavros 包也位于 ~/DASA_space/catkinws_dasa。构建工作区时，用户可以直接运行build.sh，无需再为依赖包配置环境变量。

bash build.sh

3.代码说明

3.1 px4_dasa_bridge包

与python demo类似，我们实现控制和指令接口，并将这两类接口封装到一个节点中。

Commander 类是为用户提供服务的指令接口。服务文件为DASABehaviorCmd.srv，如下所示，支持的行为可以轻松扩展。

# service for behavior commands in dasa system
# behavior ID supported in DASA system

uint8 DASA_BEHAVIOR_ID_ARM      = 0
uint8 DASA_BEHAVIOR_ID_DISARM   = 1
uint8 DASA_BEHAVIOR_ID_TURN     = 2                 # params[0]: desired yaw in ENU/DASA frame, unit: deg (user-friendly)
uint8 DASA_BEHAVIOR_ID_MOVE_TO_POSITION = 3         # params[0-2]: desired x,y,z, unit: m; params[3]: 0, DASA/ENU frame, 1, FLU frame
uint8 DASA_BEHAVIOR_ID_MOVE_VELOCITY    = 4         # params[0-2]: desired vx,vy,vz , unit: m/s; params[3]: 0, DASA/ENU frame, 1, FLU frame
uint8 DASA_BEHAVIOR_ID_LAND = 5
uint8 DASA_BEHAVIOR_ID_HOVER = 6
uint8 DASA_BEHAVIOR_ID_TRANSIT_TO_MC = 7
uint8 DASA_BEHAVIOR_ID_TRANSIT_TO_ROVER = 8
uint8 DASA_BEHAVIOR_ID_RETURN_HOME = 9              # params[0]: height above home position
uint8 DASA_BEHAVIOR_ID_SET_DRIVING_STATE = 10              # params[0]: 1, forward driving, 0, backward driving


uint8 dasa_behavior_id
float32[10] params
---
bool success

PX4Controller类是与自动驾驶仪交互的控制接口，与python demo的结构几乎相同。 MainLoopProcess() 是定期回调的主循环函数。命令模式同样支持所有 DASA 机器的位置、速度控制，以及直接控制底层驱动器信号。

void Px4Controller::MainLoopProcess(const ros::TimerEvent& event)
{
    if (_cmd_mode == PX4_CONTROLLER_NS::MC_POSITION_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_pos_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::MC_VELOCITY_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_vel_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_POSITION_ROVER_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_pos_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_VELOCITY_ROVER_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_vel_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_ACTUATOR_CONTROL_ROVER_COMMAND_MODE)
        m_fcu_actuator_control_pub.publish(m_cur_actuator_control_target);

....

3.2 offboard_demo包

offboard_demo 包更加方便用户上手。用户只需调用指令接口提供的服务即可实现各类复杂行为。需要注意的是，谨慎设置好行为参数，以避免机器出现非预期的意外动作。下面的代码展示了如何在DASA环境下操控无人机起飞，其中home_pos表示无人机的home位置。

while(ros::ok() && !flag_mission_finish){

    /*uav demo*/
    if (vehicle_type == "uav"){
        ROS_INFO("uav mission starts");

        // takeoff to 1.5 meters high
        srv.request.dasa_behavior_id = px4_dasa_bridge::DASABehaviorCmd::Request::DASA_BEHAVIOR_ID_MOVE_TO_POSITION;
        srv.request.params[0] = home_pos[0];
        srv.request.params[1] = home_pos[1];
        srv.request.params[2] = 1.5f;

        ROS_INFO("uav moves to position: %5.3f, %5.3f, %5.3f", (double)srv.request.params[0], (double)srv.request.params[1], (double)srv.request.params[2]);
        dasa_behavior_cmd_client.call(srv);

        ros::Duration(5.0).sleep();

3.2 集群案例备注

通过在 dasa.launch 文件中设置 vehicle_type 和 vehicle_id 可以轻松启用集群案例。所有相关话题和服务都会以 /vehicle_type<vehicle_id>/ 为前缀，用户可以相应地通过这些话题来实现机器行为。

4.虚拟仿真测试

仿真测试可以基于位于~/DASA_space/virtual_simulation 的DASA 虚拟环境执行。用户依次启动 mavros、px4_dasa_bridge、offboard_demo 和 vehicle SITL 节点。为方便使用，在 ~/catkinws_offboard_dasa/scripts 中提供相应脚本。

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环境中的实际飞行

操作过程与之前的python demo相同。用户可以直接使用便捷脚本，然后使用遥控器进行操作。

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