Offboard Control Example (ROS python)

The tutorial introduces an example on how to operate the flying rover with ROS python API 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 melodic with Ubuntu 18.04, or ROS kinetic with Lubuntu 16.04 in our released modules with Raspberry Pi model 3b+.

Dependencies for this tutorial are:

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

• python 3.6.9 modules:

  python3 -m pip install rospkg pyquaternion

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/offboard_demo_python
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 ~/src/flyingrover_workspace/
git clone https://github.com/cloudkernel-tech/offboard_demo_python
cd offboard_demo_python && git checkout dev_flyingrover

To build the workspace of mavros and mavlink, please refer to the C++ offboard control tutorial.

2. Code Explanation

The workspace for python offboard control is located in ~/src/flyingrover_workspace/offboard_demo_python, and it consists of two program files: commander.py and px4_mavros_run.py. The commander.py (Commander Interface) provides a user-friendly interface for application development, and px4_mavros_run.py (Control Interface) encapsulates low level message communication with mavros.

2.1 Commander Interface

The Commander Interface implements friendly API for user programming, like move(), turn(), transit_to_mc() commands. The main entry of this program provides an example for a waypoint mission in both rover and multicopter modes. We explain the main code in sequence below:

The commander class is first instantiated as an object, and the program will wait for the Control Interface to be ready so it can publish commands later on. Note that the vehicle is only able to perform required tasks when it's armed and in offboard mode.

    con = Commander()
    time.sleep(2)

    # waiting for core to be ready
    while not con.flag_core_ready:
        print("waiting for the vehicle to be armed and in offboard")
        time.sleep(1)

On initialization, the vehicle is in the rover mode. The following code will request the vehicle to go to several waypoints defined in ENU frame by default. Make sure to ajust the sleep time for driving so the vehicle can reach the desired position.

    con.move(5, 0, 0)
    time.sleep(5)
    con.move(5, 5, 0)
    time.sleep(5)
    con.move(0, 5, 0)
    time.sleep(5)
    con.move(0, 0, 0)
    time.sleep(5)

After the rover mission is completed, the program will request the vehicle to transit to the multicopter mode.

    # request to transit to multicopter
    while not con.flyingrover_mode == "MC":
        print("request to transit to multicopter mode")
        con.transit_to_mc()
        time.sleep(2)

Then the vehicle will perform the waypoint mission defined for the multicopter mode. At the end, the program will disarm the vehicle for safety.

    # multicopter mission round
    con.move(5, 0, 5)
    time.sleep(5)
    con.move(5, 5, 5)
    time.sleep(5)
    con.move(0, 5, 5)
    time.sleep(5)
    con.move(0, 0, 5)
    time.sleep(5)
    con.land()
    time.sleep(5)

    #disarm for safety at the end
    con.disarm()

2.2 Control Interface

The Control Interface implements low level communication with the mavros gateway, and there is no need to change it for most cases. The entry function is start() in the Px4Controller class, which will be called when the px4_mavros_run.py is executed.

In the start() function, the ros node is initialized first, then the program will wait for the IMU heading data to be ready so it could construct the initial target pose for vehicle command.

    rospy.init_node("offboard_node")
    rate = rospy.Rate(30)  # 30Hz

    for i in range(10):
        if self.current_heading is not None:
            break
        else:
            print("Waiting for initialization.")
            time.sleep(0.5)

    self.cur_target_pose = self.construct_target(self.local_pose.pose.position.x,
                                                 self.local_pose.pose.position.y,
                                                 self.local_pose.pose.position.z,
                                                 self.current_heading)

In simulation mode (not supported yet), the program will arm and set offboard mode for the vehicle.

    '''arm and set offboard automatically in simulation mode'''
    if flag_simulation_mode:
        print("setting arm and offboard in simulation mode...")

        for i in range(100):
            self.local_target_pub.publish(self.cur_target_pose)
            self.arm_state = self.arm()
            self.offboard_state = self.offboard()

            if self.arm_state and self.offboard_state:
                break

            time.sleep(0.05)

At last, the program will enter a while loop to respond to different commands from the Commander Interface, including position setpoints, transition commands, arm or disarm requests, etc.

   while not rospy.is_shutdown():
        # publications
        self.local_target_pub.publish(self.cur_target_pose)
        self.flyingrover_mode_pub.publish(self.current_fr_mode)
        self.landedstate_pub.publish(String(self.landed_state))

        # publish flag to indicate the vehicle is armed and in offboard
        if self.arm_state and self.offboard_state:
            self.core_ready_pub.publish(True)
        else:
            self.core_ready_pub.publish(False)
        ......

3. How to Run for Real Tests

Users are advised to follow the procedures below exactly to run the demo:

• 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/offboard_flyingrover_python/commander.py, 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/offboard_demo_python
    python3 px4_mavros_run.py
  
    # launch a new terminal
    cd ~/src/flyingrover_workspace/offboard_demo_python
    python3 commander.py

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