Offboard Control with C++ (Single Vehicle Case)

Note: This tutorial is applicable for DASA indoor configuration.

The offboard control demo is rewritten in C++ language for this tutorial. Although the ROS environment supports both Python and C++ languages, we highly recommend users familiarize with C++ because C++ is much faster than python during execution especially when users have to interface with low level hardware. The key difference with the python version is that the structure of the C++ demo can support swarm robots, as illustrated in sections below.

1. File Directory

The workspace for this demo is located in ~/DASA_space/catkinws_offboard_dasa, and it consists of two packages:

px4_dasa_bridge: the package implements the interface with px4 Kerloud autopilots for DASA vehicles.
offboard_demo: the package gives a very simple demo on waypoint mission for DASA vehicles.

2. Environment Requirements

The recommended environment is Ubuntu 18.04, ROS melodic and python 3.6.9. Software dependencies are the same with the previous python demo. The customized Mavros package is also located in ~/DASA_space/catkinws_dasa. To build the workspace, users can simply run the build.sh as below to avoid activating environment variables for dependent packages.

bash build.sh

3. Code Explanation

3.1 px4_dasa_bridge Package

Similar with the python demo, we implement control and commander interfaces, and encapsulate these two packages into one node.

The Commander class is the commander interface that provides a service for users. The service file is DASABehaviorCmd.srv as shown below, and the supported behaviors can be easily extended.

# service for behavior commands in dasa system
# behavior ID supported in DASA system

uint8 DASA_BEHAVIOR_ID_ARM      = 0
uint8 DASA_BEHAVIOR_ID_DISARM   = 1
uint8 DASA_BEHAVIOR_ID_TURN     = 2                 # params[0]: desired yaw in ENU/DASA frame, unit: deg (user-friendly)
uint8 DASA_BEHAVIOR_ID_MOVE_TO_POSITION = 3         # params[0-2]: desired x,y,z, unit: m; params[3]: 0, DASA/ENU frame, 1, FLU frame
uint8 DASA_BEHAVIOR_ID_MOVE_VELOCITY    = 4         # params[0-2]: desired vx,vy,vz , unit: m/s; params[3]: 0, DASA/ENU frame, 1, FLU frame
uint8 DASA_BEHAVIOR_ID_LAND = 5
uint8 DASA_BEHAVIOR_ID_HOVER = 6
uint8 DASA_BEHAVIOR_ID_TRANSIT_TO_MC = 7
uint8 DASA_BEHAVIOR_ID_TRANSIT_TO_ROVER = 8
uint8 DASA_BEHAVIOR_ID_RETURN_HOME = 9              # params[0]: height above home position
uint8 DASA_BEHAVIOR_ID_SET_DRIVING_STATE = 10              # params[0]: 1, forward driving, 0, backward driving


uint8 dasa_behavior_id
float32[10] params
---
bool success

The PX4Controller class is the control interface that interacts with the autopilot, and it shares nearly the same structure as the python demo. The MainLoopProcess() is the main loop function that is scheduled as a periodic callback. The command mode also supports position, velocity and direct actuator control for all DASA vehicles.

void Px4Controller::MainLoopProcess(const ros::TimerEvent& event)
{
    if (_cmd_mode == PX4_CONTROLLER_NS::MC_POSITION_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_pos_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::MC_VELOCITY_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_vel_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_POSITION_ROVER_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_pos_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_VELOCITY_ROVER_COMMAND_MODE)
        m_local_setpoint_target_pub.publish(m_cur_vel_target);
    else if (_cmd_mode == PX4_CONTROLLER_NS::ROVER_ACTUATOR_CONTROL_ROVER_COMMAND_MODE)
        m_fcu_actuator_control_pub.publish(m_cur_actuator_control_target);

....

3.2 offboard_demo Package

The offboard_demo package is very simple for users to get started. Users can just call the service provided by the commander interface to realize miscellaneous behaviors. Be cautious about the behavior parameters to avoid unexpected vehicle actions. The code below shows how to request the uav to takeoff in DASA environment, in which the home_pos denotes the home position of the uav.

while(ros::ok() && !flag_mission_finish){

    /*uav demo*/
    if (vehicle_type == "uav"){
        ROS_INFO("uav mission starts");

        // takeoff to 1.5 meters high
        srv.request.dasa_behavior_id = px4_dasa_bridge::DASABehaviorCmd::Request::DASA_BEHAVIOR_ID_MOVE_TO_POSITION;
        srv.request.params[0] = home_pos[0];
        srv.request.params[1] = home_pos[1];
        srv.request.params[2] = 1.5f;

        ROS_INFO("uav moves to position: %5.3f, %5.3f, %5.3f", (double)srv.request.params[0], (double)srv.request.params[1], (double)srv.request.params[2]);
        dasa_behavior_cmd_client.call(srv);

        ros::Duration(5.0).sleep();

3.3 Swarm Case Remark

The swarm case can be simply enabled by setting vehicle_type and vehicle_id in the dasa.launch file. Then all relate topics and services are prefixed with the /vehicle_type<vehicle_id>/ string. Users can then utilize those topics accordingly to realize vehicle behaviors.

4. Simulation Test

The simulation test can be conducted on basis of the DASA virtual environment located in ~/DASA_space/virtual_simulation. Users shall launch the mavros, px4_dasa_bridge, offboard_demo and vehicle SITL nodes in sequence. For convenience, the scripts are provided in ~/catkinws_offboard_dasa/scripts.

bash scripts/start_uav_sitldemo.sh              # uav case
bash scripts/start_flyingrover_sitl_demo.sh     # flyingrover case
bash scripts/start_rover_sitl_demo.sh           # rover case

5. Real Flights in DASA Environment

THe operation procedure is the same with the previous python demo. Users can use convenience scripts below and then operate with the transmitter.

bash scripts/run_uav_demo.sh             # uav case
bash scripts/run_flyingrover_demo.sh     # flyingrover case
bash scripts/run_rover_demo.sh           # rover case