(tutorial_offboard_cplus_zh)= # 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 分支): * 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教程中发布、订阅节点的编程方法，官方链接为：。 重点指出，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部分]()。 /*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模式，飞车将自动启动航路点任务。恭喜飞行成功！