Trajectory following with MAVROS OFFBOARD on Raspberry Pi

Jaeyoung Lim
This post is written with the work done with Modulabs DCU Lab

Introduction

Implementing a onboard computer on a small UAS can have many benefits such as being able to use ROS onboard the system. This can speed up the development process and enable to use various sensors and actuators that are supported by the ROS community. I have recently written articles on how to use MAVROS to fly a PX4 based quadrotor. However the articles do not cover operating such a system outdoors. Operating outdoors can have many issues as configuring a network or the change of localization performances and the presence of the environment. This article describes my experience on the implementation and operation of a ROS controlled MAV outdoors so that the reader can avoid the mistakes that I have made. Raspberry Pi 2 B+ was used for the onboard computer running ROS.

Results

A simple trajectory generation node was implemented to generate a circular trajectory for the quadrotor to follow. The radius was 6 m at a 15m altitude with the angular rate of 0.4 rad/s.

The video shows the comparison of the real flight with the gazebo SITL simulation. The video shows that the SITL is pretty accurate with the real flight.

Hardware Setup

Quadrotor Hardware

IMG_1039
The quadrotor is a custom built quadrotor based on the PX4 flight stack. The quadrotor has a 8X4.5 inch props with a 1200kv motor running on a 3S Lipo. Pixhawk was used for the flight controller and the quadrotor is capable of flying and basic waypoint based autonomous missions without the existence of the companion computer.

Companion Computer

Raspberry Pi 2 B+ was used for the companion computer with the pixhawk flight controller. It is not specificly a good choice to use the Raspberry Pi as a companion computer. Raspberry Pis are cheap and fun to use but using raspbian may cause build problems in some packages in ROS.

pixhawk_raspberryPi

The companion computer is stacked on top of the flight controller. The TX/RX pins are connected to the TELEM2 port on Pixhawk. From the guide Raspberry Pi is getting power from the TELEM2 port but the Raspberry Pi was keep turning off as the 5V bus on the TELEM2 port was not providing enough current. Thus, the power is provided from the 5V bus on the servo rail which is powered by a UBEC from the ESC. Powering Raspberry Pi through GPIO is not recommended as there are no regulator or backward protection. However, As I am powering the Raspberry Pi through a UBEC, it is safer.

A 5.4 wifi dongle was used for network connections.

Software configuration

softwareOverview

PX4 configuration

As the Raspberry Pi UART port only supports upto 57600 bps, the baudrate of the SYS_COMPANION parameter should be set accordingly. This is different with using the FTDI. This degrades the performance of the OFFBOARD control capabilities, but as the example on this article covers only setpoint position messages, 57600bps is enough to do the job.

SYS_COMPANION 57600

ROS configuration

Ubuntu 14.04 was used for the OS as there were some build issues using Raspbian Jessie. Ubuntu officially supports raspberry Pi, so the image was used on the raspberry Pi.
ROS indigo was used in this project. This was a decision made from the fact that ubuntu 14.04 and ROS indigo on raspberry pi was tested in a previous example.

MAVROS configuration

The name of the UART port on Raspberry Pi 2 is ttyAMA0. This should be configured in the launch file.

fcu_ur:=/dev/ttyAMA0:57600

To test run a mavros node without making a launch file you can use:

ronrun mavros mavros_node _fcu_url:="/dev/ttyAMA0:57600"

You can check if the connection with the flight controller is properly done using rostopic tools such as:

rostopic echo /mavros/imu/data

This command echos the imu data from the flight controller and prints it on the screen.

Trajectory Publisher node

rqtgraph
The node is a simple publisher publishing position setpoints to the quadrotor.

#include 
#include 
#include 
#include 
#include 
#include "math.h"

double r;
double theta;
double count=0.0;
double wn;

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise
            ("mavros/setpoint_position/local", 10);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);
    

    nh.param("pub_setpoints_traj/wn", wn, 1.0);
    nh.param("pub_setpoints_traj/r", r, 1.0);
    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
       

    theta = wn*count*0.05;

        pose.pose.position.x = r*sin(theta);
        pose.pose.position.y = r*cos(theta);
        pose.pose.position.z = 15;

    count++;

        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

There are two parameters in the launch file which are the radius and the angular rate of the quadrotor. This can be configured from the launch file.

Operatimg the quadrotor

A field box is essential to take all the necessary equipment without forgetting something. The photo below is my go-to toolbox for field tests.
IMG_20160804_131026

For operating a flight controller and a companion computer outdoors, it is convinient to configure a wifi network on the field. A wifi router is used to build a wifi network around a field which is powered by a 3S battery. A connector was built to connect the battery to the DCIn port to power a Wifi router.

IMG_1034

I used putty for the ssh client for the Raspberry Pi. The basic operation procedures in running ROS is as follows.

IMG_20160810_153257

First, the laptop should be connected to the same network as the raspberry pi is connected. Then, open a ssh session and run roscore:

roscore

Start a new session and run the launch file that has been configured. In this case:

rosrun modudculab_ros ctrl_traj_test.launch

For longer range applications, a more powerful router or antenna should be used. Using an ordinary wifi router outdoors seems to work well and simplify the operation process to make the system work.

Advertisements

8 thoughts on “Trajectory following with MAVROS OFFBOARD on Raspberry Pi

  1. Hi,

    Thank you so much for creating these tutorials on how to setup OFFBOARD control on the actual hardware platform. There are not very many out there, with the details you have included.

    I am trying to do something similar to what you did, but I am facing the issue where my copter does not receive the setpoint messages, I tried running a rostopic to see the topic but I can’t see the /mavros/imu/data topic. I am using an Odroid running ubuntu 14.04, connected on TELEM2, I can use mavproxy to intereact with my flight controller from the Odroid so I know the connection is working. Do you think you know what my issue could be ?

    Thank you

    Like

  2. Hi,

    Any idea how to rotate the quadcopter using setpoint_position/local?
    I tried ‘pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw (0.0,0.0,3.14)’ to rotate it with 180 degrees, but instead the quadcopter just slowly drifts off.

    Kind regards,
    Michiel

    Like

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s