# Offboard Control 例程 (ROS C++) 本教程涵盖内容为：基于为Kerloud flying rover系列提供的C++ API，如何在ROS中编写用于offboard控制的节点。Offboard例程使飞车在陆地车、多旋翼模式下实现航路点任务。 {% hint style="info" %} 📌 例程代码仅适于项目研究，不可直接用于产品。如果您在测试中发现错误，可随时在我们的官方资源库中创建pull请求。 {% endhint %} ## 1. 环境设置 本教程推荐的环境为Ubuntu18.04系统下ROS melodic，或者我们发行的树莓派model 3b+的ubuntu 16.04和ROS kinetic。现成可用的ROS工作区位于\~/src/flyingrover\_workspace/catkinws\_offboard目录下，该工作区包含我们官方资源库中的软件包，在维护的资源库链接如下： * mavros (dev\_flyingrover 分支): * mavlink (dev\_flyingrover 分支): * offboard control node (dev\_flyingrover 分支): 用户可通过如下指令更新到最新版本： ``` 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](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber\(c%2B%2B\))。 重点指出，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软件包的标准信息可参见：。对于飞车专用的其他话题或服务，请参阅[API部分](https://cloudkerneltech.gitbook.io/kerloud-flyingrover/user-guide/api)。 ``` /*subscriptions, publications and services*/ ros::Subscriber state_sub = nh.subscribe ("mavros/state", 5, state_cb); //subscription for flying rover state ros::Subscriber extended_state_sub = nh.subscribe ("mavros/extended_state", 2, extendedstate_cb); //subscription for local position ros::Subscriber local_pose_sub = nh.subscribe ("mavros/local_position/pose", 5, local_pose_cb); ros::Subscriber rc_input_sub = nh.subscribe ("mavros/rc/in", 5, rc_input_cb); //publication for local position setpoint ros::Publisher local_pos_pub = nh.advertise ("mavros/setpoint_position/local", 5); //service for arm/disarm ros::ServiceClient arming_client = nh.serviceClient ("mavros/cmd/arming"); //service for main mode setting ros::ServiceClient set_mode_client = nh.serviceClient ("mavros/set_mode"); //service for command send ros::ServiceClient command_long_client = nh.serviceClient ("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模式，飞车将自动启动航路点任务。恭喜飞行成功！ --- # Agent Instructions: Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://cloudkerneltech.gitbook.io/kerloud-flyingrover/user-guide-zh/tutorials_zh/offboard_cplus.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.