(tutorial_offboard_cplus)= # Offboard Control in ROS using C++ API This tutorial covers how to write an offboard control node in ROS with the provided C++ API for the Kerloud flying rover series. The offboard example is to command the flying rover for waypoint missions in both rover and multicopter modes. ```{caution} The tutorial code is for research project only, and cannot be deployed directly on a product. Feel free to create a pull request in our official repositories if you find a bug during test. ``` ## 1. Environment Setup The recommended environment for this tutorial is ROS noetic with Ubuntu 20.04, which is the default environment for the onboard Raspberry Pi model 4. The ready-to-use ROS workspace is under the directory ~/src/flyingrover_workspace/catkinws_offboard. The workspace contains the follow packages maintained in our official repositories listed below: * mavros (dev_flyingrover branch): * mavlink (dev_flyingrover branch): * offboard control node (dev_flyingrover branch): To update to latest version, users can conduct these commands as follows: 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 Due to continuous software upgrade for Kerloud products, users can create the workspace themselves in the directory ~/src/flyingrover_workspace if they don't find the code there. 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 To build the workspace, cd ~/src/flyingrover_workspace/catkinws_offboard catkin build -j 1 # set j=1 only for raspberry Pi It can take up to 10 minutes for the build process to finish in a raspberry Pi computer, so be patient for the first time. To clean the workspace, use the command: cd ~/src/flyingrover_workspace/catkinws_offboard catkin clean ## 2. Code Explanation The code example is in a standard ROS node form, and users have to familiarize with the official ROS tutorial on writing publisher and subscriber (). To highlight, the offboard_demo node contains several files: * offboard_demo/launch/off_mission.launch: a default launch file to launch the offboard control node. * offboard_demo/launch/waypoints_xyzy.yaml: waypoint mission definition file with ENU coordinates and corresponding yaw values. * offboard_demo/src/off_mission_node.cpp: the ROS node file for offboard control The mission for this example is defined as: the flying rover will firstly operate in the rover mode and run through several waypoints. After it reaches the last waypoint, the vehicle will transit to the multicopter mode, and then takeoff to fly to the same waypoints sequentially. Here we illustrate the main function of the off_mission_node in sequence for clarity. (1) Declarations of subscriptions, publications and services We usually declare ROS subscriptions, publications and services at the beginning of a main program. To obtain the state information or send commands, various topics or services from the mavros package can be easily utilized. For example, to obtain the local position information for the UAV, the mavros/setpoint_position/local topic should be subscribed. Kindly note that the coordinates are defined in the ENU (East-North-Up) frame. The standard information for the mavros package can be referred in . For other topics or sevices specifically for the flying rover, please refer to the [API section](). /*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 command construction We then construct necessary commands to call those defined services above as below. Note these commands for flying rover mode transition, and users have to understand them clearly before utilizing for real flights. /*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) Waypoint coordinate parsing for rover and multicopter modes The waypoint coordinates for rover and multicopter modes are passed to the position setpoint topic differently. For multicopter mode, the 3D position is needed, and we also have to handle the takeoff process seperately. For rover mode, the 2D position can be directly passed to the desired topic. 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 The last part of this example is to handle the mode transition for the vehicle. After it reaches the last waypoint in the rover mode, the program will call the transition service to switch to the multicopter mode. Once the transition is successful, the waypoint index is reset to zero, and then the vehicle will start takeoff and fly through the same waypoints again. /*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. How to Run for Real Tests Now it's time to take the flying rover outside for real tests. Users have to follow the orders below carefully to avoid unexpected behaviors: * Mount the battery ready with a battery cable, make sure that it will not fall down during flight. * Make sure that all switches are in their initial position and the throttle channel is lowered down. * Power on with the battery. * Wait for the GPS to be fixed: usually we can hear a reminding sound after 2-3 minutes, and the main LED in the pixhawk will turn green. * Make sure that the safety pilot is standby, and there are no people surrounding the vehicle. Wait a few minutes for the onboard computer to boot, log into it remotely via a local wifi network, confirm the waypoint mission defined in waypoints_xyzy.yaml, and run commands below to start the offboard control modules: # 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 * Arm the vehicle by lowering down the throttle and move the yaw stick to the right, then we will hear a long beep from the machine. * Switch to the OFFBOARD mode with the specified stick (e.g. channel 7), then the machine will start the waypoint mission autonomously. Cheers!