[人工智能]Kerloud 飞车在线控制C++教程

产品简介

Kerloud flying rover是由云讷科技（深圳）有限公司推出的一款面向飞车爱好者的高性价比开发平台，产品设计紧凑，兼具无人机（UAV）、无人车（UGV）模式。平台支持辅助模块扩展（树莓派、英伟达电脑），基于云讷官方提供的开源C++、Python版本SDK，用户可在室内、室外场景下快捷展开应用开发。

本教程完整链接为：https://cloudkerneltech.gitbook.io/kerloud-flyingrover/user-guide-zh/tutorials_zh/offboard_cplus

更多信息请参考官方产品网页：https://cloudkerneltech.gitbook.io/kerloud-flyingrover/

基于ROS C++ API的Offboard控制

本教程将演示如何控制飞车在陆地车、多旋翼模式下实现航路点任务。

（1）环境设置

本教程推荐的环境为Ubuntu18.04系统下ROS melodic。
现成可用的ROS工作区位于机载电脑~/src/catkinws_offboard目录下，该工作区包含我们官方资源库中的软件包，官方在维护的资源库链接如下：

• mavros: https://github.com/cloudkernel-tech/mavros
• mavlink: https://github.com/cloudkernel-tech/mavlink-gdp-release
• offboard control node: https://github.com/cloudkernel-tech/offboard_flyingrover

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

cd ~/src/catkinws_offboard/mavros
git pull origin
git checkout dev_flyingrover

cd ~/src/catkinws_offboard/mavlink
git pull origin
git checkout dev_flyingrover

cd ~/src/catkinws_offboard/offboard_flyingrover
git pull origin
git checkout dev_flyingrover

编译工作区：

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

在树莓派电脑上，此编译过程耗时可能超过十分钟，首次操作时请耐心等待。
可使用如下指令清空工作区：

cd ~/src/catkinws_offboard
catkin clean

（2）代码讲解

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

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

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

订阅、发布、服务的声明

我们通常在主程序开头处声明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");

服务指令构建

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

/*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;

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

陆地车、多旋翼模式下的航路点坐标信息，会以不同的方式传递到位置设定主题。多旋翼模式下，需要用到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);

模式切换

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

/*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();

            }

        }
    }
}

如何进行实测

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

• 将电池及线固定好，确保在飞行中不会发生脱落。

• 确保所有开关都处于初始位置，且油门通道已拉到最低。

• 电池上电。

• 等待GPS锁定：通常2-3分钟后我们可听到提示音，同时飞控上的LED会变为绿色。

• 确保飞手已就位，且飞车周围没有人。等待几分钟，机载电脑启动后可通过本地Wi-Fi网络进行远程登录，确认 /src/catkinws_offboard/src/offboard_flyingrover/launch/waypoints_xyzy.yaml目录下的航路点任务，然后通过如下指令启动offboard控制单元：

     # launch a terminal
     cd ~/src/catkinws_offboard
     source devel/setup.bash
     roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"
   
     # launch a new terminal
     cd ~/src/catkinws_offboard
     source devel/setup.bash
     roslaunch offboard_flyingrover off_mission.launch
  

• 拉低油门摇杆，同时右推偏航摇杆进行机器解锁，将听到一个长音提示。

• 使用特定的摇杆（例如通道7）切换到offboard模式，飞车将自动启动航路点任务。恭喜飞行成功！

如何购买

Kerloud无人飞车可以在云讷科技的官方淘宝店铺采购获得：

https://item.taobao.com/item.htm?spm=a1z10.1-c-s.w4004-23259098032.16.20804e895t5Ir2&id=654976354925