Offboard Control 例程 (ROS C++)

本教程涵盖内容为：基于为Kerloud flying rover系列提供的C++ API，如何在ROS中编写用于offboard控制的节点。Offboard例程使飞车在陆地车、多旋翼模式下实现航路点任务。

Caution

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

1. 环境设置

本教程推荐的环境为Ubuntu18.04系统下ROS melodic，或者我们发行的树莓派model 3b+的ubuntu 16.04和ROS kinetic。现成可用的ROS工作区位于~/src/catkinws_offboard目录下，该工作区包含我们官方资源库中的软件包，在维护的资源库链接如下：

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

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

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/catkinws_offboard/offboard_demo
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 ..
git clone --recursive https://github.com/cloudkernel-tech/offboard_demo
cd offboard demo && git checkout dev_flyingrover

编译工作区：

cd ~/src/flyingrover_workspace/catkinws_offboard/
catkin build -j 1 # set j=1 only for raspberry Pi

在树莓派电脑上，此编译过程耗时可能超过十分钟，首次操作时请耐心等待。

可使用如下指令清空工作区：

cd ~/src/flyingrover_workspace/catkinws_offboard/
catkin clean

2. 代码讲解

例程代码为标准ROS节点形式，用户需熟悉官方ROS教程中发布、订阅节点的编程方法，官方链接为：http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29。重点指出，offboard_demo节点包含以下几个文件：

offboard_demo/launch/off_mission.launch: offboard控制节点的默认启动文件。
offboard_demo/launch/waypoints_xyzy.yaml: ENU坐标系下的航路点任务定义文件，包含相应的偏航信息。
offboard_demo/src/off_mission_node.cpp: 用于offboard控制的ROS节点文件。

本例程实现的任务描述如下：飞车将先在陆地车模式下走过几个航路点，到达最后一个航路点后切换为多旋翼模式，然后起飞依次走过之前相同的航路点。为清晰起见，我们在此按顺序讲解off_mission_node中的主要功能：

(1) 订阅、发布、服务的声明

我们通常在主程序开头处声明ROS的订阅、发布、服务。通过mavros包中丰富的话题或服务，可轻松实现信息获取或命令发送。例如，若要获取无人机的本地位置信息，则需订阅mavros/setpoint_position/local话题。请注意，所用坐标系为ENU（East-North-Up）。 Mavros软件包的标准信息可参见：http://wiki.ros.org/mavros。对于飞车专用的其他话题或服务，请参阅API部分。

/*subscriptions, publications and services*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
        ("mavros/state", 5, state_cb);

//subscription for flying rover state
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>
        ("mavros/extended_state", 2, extendedstate_cb);

//subscription for local position
ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
        ("mavros/local_position/pose", 5, local_pose_cb);

ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>
        ("mavros/rc/in", 5, rc_input_cb);

//publication for local position setpoint
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_position/local", 5);

//service for arm/disarm
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
        ("mavros/cmd/arming");

//service for main mode setting
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
        ("mavros/set_mode");

//service for command send
ros::ServiceClient command_long_client = nh.serviceClient<mavros_msgs::CommandLong>
        ("mavros/cmd/command");

(2) 服务指令构建

接下来，我们将通过如下方式创建必要的命令来调用上文中所定义的服务。

/*service commands*/
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;

//flying rover mode switching commands
mavros_msgs::CommandLong switch_to_mc_cmd;//command to switch to multicopter
switch_to_mc_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_mc_cmd.request.confirmation = 0;
switch_to_mc_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::MC;

mavros_msgs::CommandLong switch_to_rover_cmd;//command to switch to rover
switch_to_rover_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_rover_cmd.request.confirmation = 0;
switch_to_rover_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::ROVER;

（3）陆地车、多旋翼模式下的航路点坐标解析

陆地车、多旋翼模式下的航路点坐标信息，会以不同的方式传递到位置设定话题。多旋翼模式下，需要用到3D位置，而且起飞过程需单独处理；陆地车模式下，则可直接将2D位置传递到所需的话题。

if (current_wpindex == 0){

    //use yaw measurement during multicopter takeoff (relative height<1) to avoid rotation, and use relative pose
    if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){

        //initialize for the 1st waypoint when offboard is triggered
        if (!is_tko_inited_flag && current_state.mode=="OFFBOARD"){

            //reload waypoint from yaml
            initTagetVector(wp_list);

            waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
            waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
            waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;

            tf::Quaternion q = tf::createQuaternionFromYaw(current_yaw);//set with current yaw measurement

            tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);

            is_tko_inited_flag = true;
        }

        //update yaw setpoint after tko is finished
        if (is_tko_inited_flag && !is_tko_finished){

            if (current_local_pos.pose.position.z >2.0f){ //reset yaw to wp_list value

                ROS_INFO("Takeoff finished, reset to waypoint yaw");

                XmlRpc::XmlRpcValue data_list(wp_list[0]);

                tf::Quaternion q = tf::createQuaternionFromYaw(data_list[3]);
                tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);

                is_tko_finished = true;
            }

        }

    }
    else //rover mode, pass relative update, use loaded waypoints
    {
        waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
        waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
        waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;
    }


    pose = waypoints.at(0);
}
else
    pose = waypoints.at(current_wpindex);

（4）模式切换

本例程的最后一部分内容是处理飞车的模式切换。当飞车在陆地车模式下到达最后一个航路点时，程序将调用模式转换服务将飞车切换到多旋翼模式。一旦切换成功，航路点索引就会被清零，飞车将会起飞并沿相同的航路点飞行。

/*mode switching or disarm after last waypoint*/
if (current_wpindex == waypoints.size()-1 && _flag_last_wp_reached){

    if (_flyingrover_mode == FLYINGROVER_MODE::ROVER){

        //request to switch to multicopter mode
        if( current_extendedstate.flyingrover_state== mavros_msgs::ExtendedState::FLYINGROVER_STATE_ROVER &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){

            if( command_long_client.call(switch_to_mc_cmd) && switch_to_mc_cmd.response.success){

                ROS_INFO("Flyingrover multicopter mode cmd activated");

                //update mode for next round
                _flyingrover_mode = FLYINGROVER_MODE::MULTICOPTER;
                current_wpindex = 0;
                _flag_last_wp_reached = false;

            }
            last_request = ros::Time::now();

        }

    }
    else if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){

        //disarm when landed and the vehicle is heading for the last waypoint
        if (current_extendedstate.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_LANDING){

            if( current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){

                if( arming_client.call(disarm_cmd) && arm_cmd.response.success){

                    ROS_INFO("Vehicle disarmed");
                    is_tko_inited_flag = false;
                    is_tko_finished = false;
                }
                last_request = ros::Time::now();

            }

        }
    }
}

3. 如何进行实测

现在可以将飞车拿到室外进行实测了。为避免意外，用户需严格按照如下流程操作：

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

确保飞手已就位，且飞车周围没有人。等待几分钟，机载电脑启动后可通过本地Wi-Fi网络进行远程登录，确认 waypoints_xyzy.yaml的航路点任务，然后通过如下指令启动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/catkinws_offboard
  source devel/setup.bash
  roslaunch offboard_flyingrover off_mission.launch

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