Offboard Control with Python (Single Vehicle Case)

Note: This tutorial is applicable for DASA indoor configuration.

Controlling a single vehicle with python language can be the starting point for entry users. The offboard control of Kerloud vehicles (UAV, flying rover and rover) in the DASA indoor environment is realized by utilizing the internal Mavros API provided in the localization workspace located in ~/DASA_space/catkinws_dasa. Note that the Mavros package is customized for DASA system to support extended vehicle operations, and standard versions in the ROS community cannot be used here.

1. File Directory

The demo program can be found in ~/DASA_space/offboard_python_dasa, and the file directory is illustrated below:

scripts/: convenience scripts for program operation, such as copying the whole directory to remote DASA agents.
commander.py: commander interface that implements various behaviors for robot operations, such as position move, arm, etc.
px4_mavros_run.py: control Interface implements low level communication with the Mavros gateway in the DASA localization workspace.
uav_demo.py: demo program to operate a Kerloud uav.
flyingrover_demo.py: demo program to operate a Kerloud flying rover.
rover_demo.py: demo program to operate a Kerloud rover.

2. Environment Requirements

Recommended environment is Ubuntu 18.04, ROS melodic and python 3.6.9

Dependencies:

mavros (dev_kerlouddasa branch): https://github.com/cloudkernel-tech/mavros
mavlink (dev_kerlouddasa branch): https://github.com/cloudkernel-tech/mavlink-gdp-release

python module requirements:

python3 -m pip install rospkg pyquaternion

We assume that the DASA localization workspace is located in ~/DASA_space/catkinws_dasa by default, and users can install other prerequisites simply by running setup.sh. All necessary prerequisites are set up in factory.

3. Code Explanation

3.1 Commander Interface

The commander interface that implements various behaviors for robot operations, such as position move, arm, etc. The reason behind is that different DASA vehicles share similar behavior interfaces in the operation level, so users can construct the application programs by simply calling those functions. For brevity, we illustrate the code in blocks below:

Initialization function:

the __init__() function initializes variables, subscriptions and publications

def __init__(self, flag_dasa=False)

Callback block:

this part declares callback function for ros subscriptions.

# callback for local position infor
def local_pose_callback(self, msg):
    self.local_position = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]

def local_velocity_callback(self, msg):
    self.local_velocity = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z]

...

Behavior block:

behavior functions for position, velocity and command requests. Please be careful with the frame definition inside. These functions support maneuvers in both ENU (East-North-Up) and DASA frames.

# move to a target position
def move_position(self, x, y, z, BODY_FLU=False):
    self.position_target_pub.publish(self.set_pose(x, y, z, BODY_FLU))

def move_velocity(self, vx, vy, vz=0, BODY_FLU=False):
    self.velocity_target_pub.publish(self.set_velocity(vx, vy, vz, BODY_FLU))

# turn to a desired yaw angle
def turn(self, yaw_degree):
    self.yaw_target_pub.publish(yaw_degree)

...

Supporting function block:

This block contains some supporting functions for pose or velocity constructions and math computations. For instance, the following function construct a waypoint pose with requested coordinates.

# set waypoint pose
def set_pose(self, x=0, y=0, z=2, BODY_FLU = False):
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()

    # ROS uses ENU internally, so we will stick to this convention
    if BODY_FLU:
        pose.header.frame_id = 'base_link'
    else:
        pose.header.frame_id = 'map'

    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.position.z = z

    return pose

3.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 will wait first for DASA frame initialization by utilizing a service call to the localization workspace, then the autopilot data must arrive before further processing.

# waiting for dasa frame initialization
if self.flag_dasa_frame:
    print("Waiting for DASA frame initialization")
    self.getDasaFrameInfo()

print("Waiting for autopilot data...")

while (self.current_heading is None or self.local_pose is None):
    time.sleep(1.0)

self.cur_pos_target = self.construct_position_target(self.local_pose.pose.position.x,
                                                     self.local_pose.pose.position.y,
                                                     self.local_pose.pose.position.z,
                                                     self.current_heading)

Then the program will arm and set offboard control mode automatically in the simulation mode, or wait for manual triggering in real flights.

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

    while True:
        self.local_target_pub.publish(self.cur_pos_target)

        if not self.arm_state:
            self.arm()

        if not self.offboard_state:
            self.offboard()

        if self.arm_state and self.offboard_state:
            print("vehicle is armed and set in offboard mode")
            break

        time.sleep(0.1)
else:
    print("Control interface: starting in real test...")

Finally, the program will enter a while loop to respond to different commands from the Commander Interface, including position setpoints, turn, hover, etc.

'''
main ROS thread
'''
while not rospy.is_shutdown():

    # publications
    if self.cmd_mode == MC_POSITION_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_pos_target)
    elif self.cmd_mode == MC_VELOCITY_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_vel_target)
    elif self.cmd_mode == ROVER_POSITION_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_pos_target)
    elif self.cmd_mode == ROVER_VELOCITY_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_vel_target)
    elif self.cmd_mode == ROVER_ACTUATOR_CONTROL_ROVER_COMMAND_MODE:
        self.actuator_control_target_pub.publish(self.cur_actuator_control_target)

    if self.vehicle_type == "flyingrover":
        self.flyingrover_mode_pub.publish(self.current_fr_mode)

    self.landedstate_pub.publish(String(self.landed_state))

...

4. Simulation Test

Operating Procedure

We highly recommend users to conduct simulation tests before real flights. The virtual simulation environment are prepared in the directory ~/DASA_space/virtual_simulation for all Kerloud vehicles. Note that simulation tests can only be performed in a personal computer.

# Attention: users have to start STIL simulation for Kerloud vehicles in virtual environment first
# uav case:
cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_uav
bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud300
# flying rover case:
# cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_flyingrover
# bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud_fr_scorpion
# rover case:
# cd ~/DASA_space/virtual_simulation/sitl_space_kerloud_rover
# bash sitl_run.sh $PWD/bin/px4 none gazebo kerloud_rover

# terminal 1
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# launch the mavros node for simulation
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# terminal 2: start the commander interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# start demo programs for a specific vehicle type
python3 uav_demo.py --flag_dasa False
# python3 flyingrover_demo.py --flag_dasa False
# python3 rover_demo.py --flag_dasa False

# terminal 3: start the control interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle uav
# python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle flyingrover
# python3 px4_mavros_run.py --sim True --flag_dasa False --vehicle rover

Or simply use convenience 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

Operating Procedure

Users are advised to follow the procedures below exactly to run the demo. Keep in mind that safety comes first, and a safety pilot should be at presence for every flight.

Step 1: Mount the battery ready with a battery cable, make sure that it will not fall down during flight.

Step 2: Make sure that all switches are in their initial position and the throttle channel is lowered down.

Step 3: Turn on the DASA wifi router with the network name DASA_swarm, and also those UWB devices in the experiment setup. Users shall configure the DASA indoor localization system properly according to the user manual.

Step 4: Make sure that the safety pilot is standby, and there are no people surrounding the vehicle. Power on the vehicle, and wait a few minutes for the onboard computer to boot. The DASA system provides a ready-to-use DASA_swarm network that all Kerloud vehicles can automatically connect on boot. For each vehicle, a unique ID is configured in its hostname.

To check whether a vehicle with ID x is connected to the local network, simply ping its ip or hostname in the master computer.

ping <vehicle ip address: 192.168.0. 100+x> or ping ubuntu<x>

The vehicle can only be remotely operated if replies can be returned from the target ip address.

Step 5: We have to synchronize the python workspace to the remote machine, simply run the script in scripts/ folder:

bash scripts/sync_python_demo_remote.sh

Step 6: Log into the remote vehicle using the ssh command:

ssh ubuntu@<vehicle ip address: 192.168.0. 100+x>  or ssh ubuntu@ubuntu<x>

and then perform the following commands. Note that localization nodes have to be brought up before we run the python demo code.

# terminal 1: start DASA localization nodes
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/catkinws_dasa && source devel/setup.bash
# launch nodes for dasa localization
bash scripts/single_case/<run_script>.sh

# terminal 2: start the control interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
python3 px4_mavros_run.py --sim False --flag_dasa True --vehicle <vehicle_type>

# terminal 3: start the commander interface
# source the mavros workspace in dasa locolization workspace
cd ~/DASA_space/offboard_python_dasa
source ../catkinws_dasa/devel/setup.bash
# start demo programs for a specific vehicle type
python3 uav_demo.py --flag_dasa True
# python3 flyingrover_demo.py --flag_dasa True
# python3 rover_demo.py --flag_dasa True

# then users can arm and set offboard mode with a transmitter to start the mission

Or simply use convenience scripts 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

6. Demonstrations

Offboard control simulation for DASA vehicles: