Offboard Control in ROS using python API

The tutorial introduces an example on how to operate the Autocar with ROS python API in various commanding modes.

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/offboard_demo_python, and the dependencies are as follows:

To update to latest version, users can conduct these commands as follows:

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/offboard_demo_python
git pull origin
git checkout dev_rover

To build the workspace of mavros and mavlink, please refer to the previous C++ offboard control tutorial. The python code doesn’t need to be built.

2. Code Explanation

The workspace for python offboard control is located in ~/src/rover_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. Users are advised to read through the README file.

2.1 Commanding Interface

The Commander Interface implements friendly API for user programming, like move_position(), move_velocity() commands. The main entry of this program provides an example for operating the rover in various commanding modes. The commanding modes are the same with those in the previous C++ offboard control tutorial. 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.

# initialize the commanding interface
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 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.

# position guidance cmd
print("Commander interface: start position control mode")
con.move_position(5, 0, 0)
time.sleep(5)
con.move_position(5, 5, 0)
time.sleep(5)

Then the vehicle will enter the velocity guidance mode, and track the desired velocity defined in the ENU frame by default. Note that the API also supports velocity tracking in the FLU (Front-Left-Up)frame.

# velocity guidance cmd
print("Commander interface: start velocity control mode")
con.move_velocity(0.3, 0.3, 0.3, False)
time.sleep(5)

Afterwards, the vehicle enters the attitude guidance mode and direct actuator control mode. In the attitude guidance mode, users can set a yaw setpoint and a desired throttle value. In the direct actuator control mode, users can set the desired actuator inputs for steering servo and main motor. These APIs can be used for advanced motion operations such as trajectory tracking, aggressive maneuvers, etc.

# attitude guidance cmd
print("Commander interface: start attitude control mode")
print("set forward driving")
con.set_forward_driving()
con.set_yaw_and_throttle(3.14/2, 0.3)
time.sleep(5)

# direct actuator control cmd
print("Commander interface: start actuator control mode")
con.set_steering_servo_and_throttle(0.5, 0.3)
time.sleep(5)
con.set_backward_driving()
print("set backward driving")
con.set_steering_servo_and_throttle(0.5, 0.3)

Finally, the vehicle will be disarmed.

#disarm for safety at the end
print("Commander interface: disarm vehicle, mission completed")
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("Control interface: waiting for initialization.")
        time.sleep(0.5)

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

In simulation mode, 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_pos_target)
        self.arm_state = self.arm()
        self.offboard_state = self.offboard()

        if self.arm_state and self.offboard_state:
            break

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

At last, the program will enter a while loop to respond to different commands from the Commander Interface, including position setpoints, velocity setpoints, forward/backward driving setting command, arm or disarm requests, etc.

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

    # publications

    if self.cmd_mode == POSITION_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_pos_target)
    elif self.cmd_mode == VELOCITY_ROVER_COMMAND_MODE:
        self.local_target_pub.publish(self.cur_vel_target)
    elif self.cmd_mode == ATTITUDE_ROVER_COMMAND_MODE:
        self.attitude_target_pub.publish(self.cur_attitude_target)
        self.thrust_target_pub.publish(self.cur_thrust_target)
    elif self.cmd_mode == ACTUATOR_CONTROL_ROVER_COMMAND_MODE:
        self.actuator_control_target_pub.publish(self.cur_actuator_control_target)

    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

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.

  • Log into the onboard computer remotely in a local network, and confirm the mission defined in the commanding interface.

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

# terminal 1
# source the mavros package in the c++ offboard ros workspace
cd ~/src/rover_workspace/offboard_demo_python
source ../catkinws_offboard/devel/setup.bash
# launch the mavros node for real tests
roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"

# terminal 2
cd ~/src/rover_workspace/offboard_demo_python
source ../catkinws_offboard/devel/setup.bash
python3 px4_mavros_run.py

# terminal 3
cd ~/src/rover_workspace/offboard_demo_python
source ../catkinws_offboard/devel/setup.bash
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 mission autonomously. Cheers!