2. Coding UAVs with ROS. Subscribing to FCU data.

What’s this about?

In this post we will build a ROS node on a companion computer to subscribe to data being published by the flight control unit (FCU).  This will allow us to use the many data streams available from the flight controller as inputs to our system and then be able to make decisions over how the UAV should be controlled.

Prerequisites

This post follows on from the previous post 1. Coding UAVs with ROS. SetUp using the Ubuntu image published by the wonderful folk at Ubiquity Robotics.

We are using a Pixhawk FCU running the PX4 flight stack.  This is connected to a Raspberry Pi 3 companion computer running Ubuntu, with ROS installed and specifically the MAVROS package to facilitate communication between the FCU and companion computer.

Nodes, Topics and Messages

This is a very quick introduction!

In ROS, data is essentially communicated throughout the system using a publisher/subscriber model.

  1. Nodes publish and subscribe to available data streams and process that data.  The MAVROS node for example, communicates with the Pixhawk FCU and publishes the data it receives.  We are going to create a node that subscribes to data published by the MAVROS node.
  2. Topics are the data streams available between nodes.  Typically each topic ‘encapsulates’ some meaningful data, for example the attitude of the vehicle.  We are going to subscribe to the topic published by the MAVROS node that contains the current flight state of the FCU (so MANUAL, OFFBOARD etc.).  As you saw in the previous tutorial, data in topics is published at a particular frequency, for example 1Hz.
  3. Messages are the data items themselves contained in topics.  So we subscribe to a topic and then read the messages as they are published.  Typically a message will comprise a header with a timestamp, and then the data itself, which might be very simple such a string, or more complex such as position data (x,y,z) and orientation data (roll, pitch, yaw).

As a visual learner, I envisage the system as a system of pipes (topics), connecting junctions (nodes).  Inside the pipes are the messages themselves being passed between the nodes. The great thing is that there is a utility built into ROS that allows us to see this structure, namely rqt_graph, which we will use extensively.

In fact, here the ROS graph of the code we are building.  It shows the /FCU_state_monitor node subscribing to the /mavros/state topic, which is published by the /mavros node.  (Ignore the /tf_static topic).

Screenshot from 2018-08-15 10-00-04

Create a ROS package

We are going to create a package for our files inside the catkin_ws workspace, in the src directory.  This will contain all our source code, data and documentation in one place.

  1. Power up the Raspberry Pi companion computer and login (the default password is ubuntu).
  2. In a terminal in the src directory of folder catkin_ws:
    • $ catkin_create_pkg fcu_monitor rospy
      (Nb. The argument rospy is a dependency of the package.Screenshot from 2018-08-13 21-50-26
  3. Open the folder fcu_monitor created in /catkin_ws/src
    It contains folders for the code and other information for the package.
    We will create our Python code in the /fcu_monitor/src folder.Screenshot from 2018-08-15 10-08-22
    Screenshot from 2018-08-15 10-08-53

Create the Python file

  1. In the /fcu_monitor/src folder, right click and create an empty file called FCU_monitor.pyScreenshot from 2018-08-15 10-10-13
  2. Make the file executable.  I prefer to use the GUI for this  – right click, File Properties, Permissions.Screenshot from 2018-08-15 10-12-28
  3. Open the file for editing.  For some reason, IDLE does not seem to work (for me at least) on the image of Ubuntu Mate supplied by Ubiquity Robotics.  No matter – just use the GNU Emacs 24 (GUI) editor instead.  This is a little more complex to use – I dismiss the startup screen below the main editing area.  BUT remember to save using the Save icon on the menu bar and not just use ctrl-S.  This actually posts the changes made to the file and saves it.  Emacs also sets up a buffer file of changes alongside the source code file, which can be ignored for now.Screenshot from 2018-08-15 10-17-29
    Screenshot from 2018-08-15 10-23-06

Let’s Get Coding

We are going to create a class which defines a node to subscribe to the  /mavros/state topic.  The node has no idea what other node is publishing the topic and it doesn’t need to.  Let’s look at all the code, then work through it.

Overview

Screenshot from 2018-08-15 10-31-47

First we import the key Python libraries required to run the code – sys and rospy.  These are always included (for Python code).

Screenshot from 2018-08-15 10-31-47_1

The class needs to be able to interpret the message type within the  /mavros/state topic.  The next two lines import the definitions required to do this. The detail is easily looked up using ROS command line utilities, which we will go through together in the next section.

The class itself is standard Python.  We have a function to initialise an instance of the class and further functions as required.  We initialise the node (FCU_sub) to subscribe to the topic which calls a further function every time a message is received on that topic (FCU_callback).

The main program simply creates an instance of the node and handles the shutdown gracefully.

Detail

Before we can create a node, we need to drill down understand the message types contained within the topic(s) we are subscribing to or publishing.

We can start to do this by running the (other) nodes and examining the topics they are publishing.

  • Open a terminal (anywhere) and start roscore:
    $ roscore
  • Open another terminal (anywhere) and launch the mavros node:
    $ roslaunch mavros px4.launch

So mavros should now be publishing a whole bunch of topics based on data it is receiving from the FCU.  Let’s take a look at them.

In another new terminal:

  • $ rostopic list
    Screenshot from 2018-08-15 10-59-41_1

The topic to which we wish to subscribe is /mavros/state.  We need to know what message types it contains.  In the same or a new terminal:

  • $ rostopic info /mavros/state
    Screenshot from 2018-08-15 11-25-05_1

We can see (2nd line above) the type of the messages in the topic is mavros_msgs/State.  This allows us to define our first type import:

Screenshot from 2018-08-15 10-31-47_2

Now we need to see the details of the message type, to pick out the data types of any content we wish to work with. In the same or a new terminal:

  • $ rosmsg info mavros_msgs/State
    Screenshot from 2018-08-15 11-33-13_1

We can see the message comprises a header with a timestamp, some Booleans and a string containing the mode – it’s that we are after.  The type ‘string’ is a basic type within the ROS std_msgs library, so we import it from that.  Leave this terminal window open for now.

Screenshot from 2018-08-15 10-31-47_3

OK – so we have imported the libraries we need and key message types.  Now to the build the node itself.

First we specify the name of the node – ‘FCU_state_monitor‘.  Obviously keep it meaningful (it’s the name that will also appear on the ROS graph).

Then we set it up as a node to subscribe to the topic /mavros/state, specifying the topic type State to match that imported above.  Finally we specify the function to be called when each new message arrives – here it’s self.FCU_callback.

Screenshot from 2018-08-15 10-31-47_5

I like to store key data within the class so that is accessible across an instance of the class, for example to be used by another function when some new data arrives from a different topic.  Here we initialise a variable self.FCU_status to store the flight state mode.

Screenshot from 2018-08-15 10-31-47_6

Now we define the callback function, called when new a message of the /mavros/state topic arrives.  Note the name must match that specified when the subscriber node was set up above. The message is passed to the function in the argument, here imaginatively called msg, but you can call it what you like.

Screenshot from 2018-08-15 10-31-47_7From our rosmsg utility used earlier, we can see the data item we want is called mode and it’s at the top level of the message.  So we can refer to the data item as msg.mode.

So here we store the value of  msg.mode into our variable self.FCU_status and then simply print it out to the terminal.
Screenshot from 2018-08-15 10-31-47_8The main program simply creates an instance of the node, called sm (state monitor).

Screenshot from 2018-08-15 10-31-47_9The question is then what happens?  In a ‘traditional’ polling approach, execution would follow through to the creation of the subscribing node and then wait until the first message arrives.  This would block further execution and obviously be fairly disastrous.  ROS however, uses a special version of  Python’s ‘sleep’ called rospy.spin that simply suspends the execution of the particular node(s) belonging to the class until a new message arrives.  This way the execution of other nodes in the system is unaffected and (robot) life goes on.  It’s very clever stuff with a bit of threading behind the scenes, but it’s also what makes ROS such a great platform for complex robotics, amongst other features.

Run the Code

  1. As usual, run roscore and launch the mavros node in new terminals if they are not already running.
    • $ roscore
    • $ roslaunch mavros px4.launch
  2. We are running the code directly within the package itself for simplicity, so make sure you open a terminal within the folder containing the Python code, namely ~/catkin_ws/src/fcu_monitor/src.  You are probably already there, of course.
  3. Open the terminal and run the Python file:
    • $ rosrun FCU_monitor.py
      Screenshot from 2018-08-16 17-49-13_1

The flight mode is printed out every second, or 1Hz.  This is because mavros publishes the data from the FCU at 1Hz, so the callback function is invoked at 1Hz..  Different topics are published at different rates, which can also be changed (on the FCU side).

Leave the new node running…

Examining the Node

  1. Verify the node is running (in a new terminal):
    • $ rosnode list
    • The node /FCU_state_monitor is listed with /mavros. Great!
      Screenshot from 2018-08-16 17-51-56
  2. Take a look at more information about the node:
    • $ rosnode info
    • Amongst other information, we can see the node subscribes to the topic /mavros/state
      Screenshot from 2018-08-16 17-54-37_1
  3. Finally, in a new terminal, show the graph of the current system of running nodes:
    • $ rqt_graph
    • (it may take a moment and ignore any component authentication errors)
      Screenshot from 2018-08-15 10-00-04

Summary

We have built a simple subscriber to read data from the FCU, via mavros.  In doing so, we:

  • Created a simple package.
  • Started ROS with roscore and launched mavros.
  • Listed running nodes and displayed information about them to be able to subscribe to published topics.
  • Built our own subscriber node.
  • Displayed information about our new node.
  • Learned how to show a graph of running nodes using rqt_graph.

Github

Code for this post is on GitHub here.

Extension

The purpose of nodes is (usually) to perform some operations on data from topics they subscribe to and then publish data through topics of their own.

Here are some suggestios of thing you might like to try now:

  • Extract further data from the messages received by the /FCU_state_monitor node.  Combine these with a little logic to print something like ‘Armed and in Manual mode’ or similar.
  • Add a further subscriber within the same class to a different topic.  Combine data from these topics in some way.

 

 

 

Author: Mike Isted

A multi-disciplinary technologist. Embedded software engineer.