Offboard Control Example (ROS C++)

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.

📌 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): https://github.com/cloudkernel-tech/mavros

• mavlink (dev_flyingrover branch): https://github.com/cloudkernel-tech/mavlink-gdp-release

• offboard control node (dev_flyingrover branch): https://github.com/cloudkernel-tech/offboard_demo

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 (http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29).

To highlight, the offboard_flyingrover 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 http://wiki.ros.org/mavros. 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_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 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 ~/src/catkinws_offboard/src/offboard_flyingrover/launch/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!