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 Autocar series. The offboard example can serve as a basis to develop applications for various tasks.

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 is ROS kinetic with Ubuntu 16.04, although this tutorial can run in other ROS versions.

The ready-to-use ROS workspace is under the directory ~/src/rover_workspace/catkinws_offboard. The workspace contains the follow packages maintained in our official repositories listed below:

To update to the latest version, users can conduct the following commands:

cd ~/src/rover_workspace/catkinws_offboard/mavros
git pull origin
git checkout dev_rover

cd ~/src/rover_workspace/catkinws_offboard/mavlink
git pull origin
git checkout dev_rover

cd ~/src/rover_workspace/catkinws_offboard/offboard_mission
git pull origin
git checkout dev_rover

To build the workspace,

cd ~/src/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/rover_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 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

2.1 Commanding Modes

The rover can be operated in several commanding modes:

  • Position Mode: The rover can be guided towards a position setpoint in forward driving.

  • Velocity Mode: The rover can follow a velocity setpoint in forward driving.

  • Attitude Mode: The rover can be operated with an attitude setpoint and a desired motor throttle value. Both forward and backward driving are supported.

  • Actuator Control Mode: The rover can be operated with direct actuator controls including steering servo and motor throttle inputs. Both forward and backward driving are supported.

2.2 Program Clarification

The mission for the offboard demo node consists of four phases corresponding to above commanding modes in sequence. The key is to utilize various topics or services provided in the C++ API.

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

 /*subscriptions, publications and services*/
 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 5, state_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);

//publication for local velocity setpoint
ros::Publisher local_vel_pub = nh.advertise<geometry_msgs::Twist>
        ("mavros/setpoint_velocity/cmd_vel", 5);

//publication for attitude setpoint (attitutde & thrust)
ros::Publisher att_sp_pub = nh.advertise<geometry_msgs::PoseStamped>
        ("mavros/setpoint_attitude/attitude", 5);

ros::Publisher thrust_sp_pub = nh.advertise<mavros_msgs::Thrust>
        ("mavros/setpoint_attitude/thrust", 5);

(2) Service command construction

We then construct necessary commands to call those defined services as below, including arm/disarm, main mode setting, and forward/backward driving request.

//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");

(3) Position guidance phase

In the position guidance phase, the rover will load the waypoint mission defined in waypoints_xyzy.yaml, and then go through these waypoints sequentially. Waypoint index is updated when the rover reaches a certain point.

/* publish local position setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::POS_PHASE:{

        ROS_INFO_ONCE("Starting position guidance phase");

        geometry_msgs::PoseStamped pose; //pose to be passed to fcu

        if (current_wpindex == 0){
            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);

        local_pos_pub.publish(pose);
        updateWaypointIndex();

    break;
}

(4) Velocity guidance phase

After all waypoints in the position guidance phases are reached, the rover will switch to the velocity guidance phase. The velocity setpoint is defined in the ENU frame, and the autopilot will handle the velocity setpoint in the body frame by decomposing the velocity into a forward speed and a lateral speed. The forward speed setpoint will be used to calculate the desired motor throttle under a PID control law, and the lateral component is employed to derive the steering angle.

 /* publish local velocity setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::VEL_PHASE:{

        ROS_INFO_ONCE("Starting velocity guidance phase");

        //The local velocity setpoint is defined in the ENU frame, and will be converted to body frame in the autopilot for maneuvering
        geometry_msgs::Twist  vel_cmd;
        vel_cmd.linear.x = 0.25f;
        vel_cmd.linear.y = 0.25f;
        vel_cmd.linear.z = 0.0f;

        local_vel_pub.publish(vel_cmd);

        //phase transition after a certain time
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(6.0)){
            _mission_phase = ROVER_MISSION_PHASE::ATTITUDE_PHASE;
            _phase_entry_timestamp = ros::Time::now();
        }

        break;

}

(5) Attitude control phase

In the attitude control phase, users can construct a attitude setpoint with a desired yaw angle, and provide a throttle value. The autopilot will handle the attitude tracking with steering servo actuation.

 /* publish attitude setpoint for rover maneuvering*/
case ROVER_MISSION_PHASE::ATTITUDE_PHASE:{
        ROS_INFO_ONCE("Starting attitude maneuvering phase");

        //we need to construct both attitude and thrust setpoints
        //we construct desired attitude from yaw setpoint
        geometry_msgs::PoseStamped pose;

        tf::Quaternion q = tf::createQuaternionFromYaw(M_PI/2.0f); //yaw unit: radius

        tf::quaternionTFToMsg(q, pose.pose.orientation);

        att_sp_pub.publish(pose);

        mavros_msgs::Thrust thrust_sp;
        thrust_sp.thrust = 0.3f;
        thrust_sp_pub.publish(thrust_sp);

        //phase transition after a certain time
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(6.0)){
            _mission_phase = ROVER_MISSION_PHASE::ACT_CONTROL_PHASE;
            _phase_entry_timestamp = ros::Time::now();
        }

    break;
}

(6) Actuator control phase

In this phase, users can directly control the low level actuators (steering servo and motor) to achieve more sophisticated tasks like path planning, trajectory tracking.

 /* publish direct actuator control for rover maneuvering*/
case ROVER_MISSION_PHASE::ACT_CONTROL_PHASE:{
        ROS_INFO_ONCE("Starting direct actuator control phase");

        mavros_msgs::ActuatorControl act_control;

        act_control.group_mix = 0;
        act_control.flag_rover_mode = 1;//enable direct actuator control in rover
        act_control.controls[mavros_msgs::ActuatorControl::ROVER_YAW_CHANNEL_CONTROL_INDEX] = 0.4f;
        act_control.controls[mavros_msgs::ActuatorControl::ROVER_THROTTLE_CHANNEL_CONTROL_INDEX] = 0.3f;

        act_controls_pub.publish(act_control);

        //we switch to backward driving after 5s
        if ( ros::Time::now() - _phase_entry_timestamp > ros::Duration(5.0)){

            if (flag_forward_driving && (ros::Time::now() - last_request > ros::Duration(1.0))){

                if( command_long_client.call(set_backward_driving_cmd) && set_backward_driving_cmd.response.success){

                    ROS_INFO("Driving state switching cmd sent");

                    flag_forward_driving = false;

                }

                last_request = ros::Time::now();

            }


        }

3. How to Run for Real Tests

Now it’s time to roll out for real tests. Since the Autocar is mainly for indoor applications, it’s a prerequisite to launch localization modules before this offboard control tutorial. For instance, we have to launch the laser localization algorithm for the Autocar Laser. Users then can follow orders below to avoid unexpected behaviors:

  • Make sure that all switches in the transmitter are in their initial position and the throttle channel is lowered down.

  • Power on with the battery.

  • 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.

  • Launch the localization modules (such as laser slam) first, and validate the position output via rostopic command.

  • Run commands below to start the offboard control modules:

# launch a terminal
 cd ~/src/rover_workspace/catkinws_offboard
 source devel/setup.bash
 roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

 # launch a new terminal
 cd ~/src/rover_workspace/catkinws_offboard
 source devel/setup.bash
 roslaunch off_mission 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 mission autonomously. Cheers!