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:
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
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.
(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!