Offboard Control with Mavros (Python)

Note: This tutorial is applicable for all TAObotics Q series products.

The offboard control with mavros in python language is conducted in the same fashion as the C++ case. The idea is to utilize standard topics or services in the mavros package to retrieve vehicle information or send commands. The workspace can be found in ~/src/uav_workspace/pythonuav_ros, and all necessary prerequisites are set up in factory.

Environment Setup

Dependencies for this tutorial are:

  • mavros: standard mavros package with different ROS versions, which can be installed by: sudo apt install ros--mavros

  • mavlink: standard mavlink package with different ROS versions, which can be installed by: sudo apt install ros--mavlink

Users can refer to the README file in the workspace for details.

We also maintain customized mavros & mavlink packages in github for compatibility concern, which can be referred in:

To use these packages, users have to create a new catkin workspace, and need to source the environment by source devel/setup.bash each time when they want to launch these customized nodes.

Code Explanation

The workspace 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.

Commander Interface

The Commander Interface implements friendly API for user programming, like move(), turn(), hover(), land() commands. The main entry of the interface is as follows:

if __name__ == "__main__":

    # the mission performs a square waypoint flight with position target defined in FLU frame
    con = Commander()
    time.sleep(2)
    con.move(5, 0, 0)
    time.sleep(5)
    con.move(0, -5, 0)
    time.sleep(5)
    con.move(-5, 0, 0)
    time.sleep(5)
    con.move(0, 5, 0)
    time.sleep(5)
    con.land()
    time.sleep(5)

The commander class is first instantiated as an object, and then sleeps for 2 seconds. The program will request the vehicle to go to several waypoints defined in ENU frame by default. Make sure to ajust the sleep time so the vehicle can reach the desired position. Users can edit the entry function easily by adding more UAV motions such as hover, turn, etc.

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. The program will first command the vehicle to takeoff to a desired height (3.2m by default).

rospy.init_node("offboard_node")
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.takeoff_height,
                                             self.current_heading)

Then the program will arm and set offboard control mode automatically:

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

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

while self.arm_state and self.offboard_state and (rospy.is_shutdown() is False):

    self.local_target_pub.publish(self.cur_target_pose)

    if (self.state is "LAND") and (self.local_pose.pose.position.z < -1.0):

        if(self.disarm()):

            self.state = "DISARMED"


    time.sleep(0.05)

How to Run for SITL Simulation

We highly recommend users to conduct SITL simulation test before real flights. The setup procedure of SITL environment can be referred in the previous C++ offboard control tutorial .

Assuming that the firmware repo is in the directory ~/src/Firmware, the gazebo-based SITL simulation can be started with:

# launch a new terminal
cd ~/src/Firmware
make px4_sitl_default gazebo

# launch a new terminal
roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

# launch a new terminal
cd ~/src/uav_workspace/pythonuav_ros
python3 px4_mavros_run.py

# launch a new terminal
cd ~/src/uav_workspace/pythonuav_ros
python3 commander.py

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/uav_workspace/pythonuav_ros/commander.py, and run commands below to start the offboard control modules:

# launch a terminal (users can source customized mavros & mavlink workspace alternatively)
roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

# launch a new terminal
cd ~/src/uav_workspace/pythonuav_ros
python3 px4_mavros_run.py

# launch a new terminal
cd ~/src/uav_workspace/pythonuav_ros
python3 commander.py
  • The vehicle will takeoff first and carry out the defined mission autonomously. Be sure to switch to the POSITION mode if any unexpected behavior happens.

results matching ""

    No results matching ""