Getting started with MAVSDK – Java

Getting started with MAVSDK-Java

Author: Jonas Vautherin | Auterion Software Engineer and MAVSDK core maintainer

This article will explain the basics to get started with MAVSDK-Java (similar to the one I wrote a few months ago about MAVSDK-Python). Once again, no particular knowledge of MAVSDK or MAVLink is expected here. We will first look at the simulation environment, then get started with MAVSDK-Java, and finally we will learn what it implies to use MAVSDK-Java on Android.

SITL simulator

Having a simulator is a safe and efficient way to start working with MAVSDK. I personally use two different ways in my day-to-day work, that I will describe below: I’ll call them “the quickstart way” and “the toolchain way”. Before going into that, let’s just mention a few definitions:

  • “SITL” stands for “software in the loop”, which means that the drone is completely simulated on your computer (find more information here).
  • “HITL” stands for “hardware in the loop”. This happens when you run the firmware (PX4) on the hardware (e.g. directly in the drone), but simulate the sensor inputs from the computer. In this setup, your drone believes that it is flying, while it is actually just sitting on your desk. We won’t go into more details about HITL in this post.
  • There are multiple simulators: gazebo, jMAVSim, AirSim, and others. In this post we will talk about gazebo and jMAVSim. From the MAVSDK perspective, it makes no difference: our code talks to a MAVLink drone, and does not care whether it is simulated or not.
  • mavsdk_server is the component that talks to the drone with MAVLink and that exposes an interface to language bindings, in this case MAVSDK-Java. This means that MAVSDK-Java talks to mavsdk_server, which in turn talks to the drone.

The quickstart way

Leveraging docker, this allows you to run a headless instance of gazebo in one command:

docker run --rm -it jonasvautherin/px4-gazebo-headless:1.10.1

It works on Linux, Windows and macOS, and options are documented in the README. Note that there may be small tricks for Windows and macOS, described in the troubleshooting section.

When working with a headless simulator, I usually like to run QGroundControl in parallel, so that I get some graphical output.

The toolchain way

In order to run SITL with the graphical interface, you need to build the PX4 toolchain from sources (the docker container does it for you, but has to be headless). It is not as straightforward as running px4-gazebo-headless above, but the good news is that it is much easier than it used to be! You’ll find instructions in the PX4 dev guide.

Once you reach the point where you can run make px4_sitl jmavsim, you should end up with a graphical view like this:

Taking off in jMAVSim

jMAVSim after a successful takeoff.

Understanding the simulator

Once SITL is started, you get a terminal and optionally a graphical view of the drone (if you don’t run SITL in a headless way). In the terminal, you get output similar to:

INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO [logger] logger started (mode=all)
INFO [logger] Start file log (type: full)

If you hit , you’ll see the “pxh” prompt appear:

pxh>

This prompt can be used to interact with the simulator. For instance, I always like to take off and land to make sure that my drone is ready:

pxh> commander takeoff
pxh> commander land

It could be that the command is refused, and that usually happens before the drone acquires a GPS fix. For instance, you’ll get used to the fact that the drone does not have a fix before you see those two lines:

INFO [ecl/EKF] 728000: EKF GPS checks passed (WGS-84 origin set)
INFO [ecl/EKF] 5196000: EKF commencing GPS fusion

Taking off with MAVSDK-Java

Running mavsdk_server

It is important to realize that MAVSDK is made of two parts: mavsdk_server and the language bindings (here we are interested in MAVSDK-Java). This means that next to the simulator, you need to run mavsdk_server, which you can either build from sources or download from the releases artifacts. On Linux, I would typically download “mavsdk_server_manylinux1-x64” and run it like so (note that I set the listening port to 50051, we will come back to it later):

./mavsdk_server_manylinux1-x64 -p 50051

If SITL is running and the connection is successful, you should see an output similar to:

“[11:42:28|Info ] MAVSDK version: 0.24.0 (mavsdk_impl.cpp:25)”
“[11:42:28|Debug] New: System ID: 0 Comp ID: 0 (mavsdk_impl.cpp:399)”
“[11:42:28|Info ] Server started (grpc_server.cpp:38)”
“[11:42:28|Info ] Server set to listen on 0.0.0.0:50051 (grpc_server.cpp:39)”
“[11:42:28|Info ] Waiting to discover system on udp://:14540… (connection_initiator.h:22)”
“[11:42:32|Info ] New system on: 172.17.0.2:14580 (udp_connection.cpp:264)”
“[11:42:32|Debug] Component Autopilot (1) added. (system_impl.cpp:352)”
“[11:42:32|Debug] Discovered 1 component(s) (UUID: 5283920058631409231) (system_impl.cpp:525)”
“[11:42:32|Info ] System discovered [UUID: 5283920058631409231] (connection_initiator.h:61)”

Notice how it says that mavsdk_server is listening on “50051”, how it is waiting for a MAVLink system on “udp://:14540”, and how a new system is discovered on “172.17.0.2”, which is a docker IP. This tells us that I was running SITL from the docker container.

We are almost ready to run code! Because I’m running px4-gazebo-headless, I like running QGroundControl as well, so that I get the state of the drone and I can see if my code works correctly.

QGroundControl connected to the simulator

QGroundControl connected to SITL. Notice how the drone is “Disarmed”, at altitude “0.0” (i.e. on the ground).

Running in a REPL

I personally like being able to running code interactively in a read-eval-print-loop (REPL), and I find it a nice tool for learning. Here we will use jshell. It may seem a bit weird to start (as compared to the Python REPL), but it works. Note that it requires Java 9+: in case you compile jMAVSim from sources, you may have to deal with jMAVSim requiring Java 8 and jshell requiring 9+.

Let’s start by cloning MAVSDK-Java and navigate into “java-client/”:

git clone https://github.com/mavlink/mavsdk-java --recursive
cd mavsdk-java/examples/java-client

We now need to fetch the dependencies of java-client, so that they can be imported into jshell:

./gradlew exportDeps

Once this is done (it has to be done only once), jshell can be started with the following command:

jshell --class-path $(ls -d build/jshell_classpath_export/* | tr '\n' ':')

You should get a REPL:

jshell>

We can now import MAVSDK-Java classes, and we want to start with System, which is our entrypoint to MAVSDK:

jshell> import io.mavsdk.System

Then we can instantiate a System, which in our case is a drone. MAVSDK calls it “system” because there are other MAVLink systems, but here we will just consider a drone.

jshell> System drone = new System()
drone ==> io.mavsdk.System@34f6515b

The second line shows that drone was assigned a io.mavsdk.System object. That’s just some REPL output.

One interesting thing to note here is that we instantiate a new System(), which is the equivalent of new System("127.0.0.1", 50051). It means that this drone object will connect to a mavsdk_server instance running on “127.0.0.1” (i.e. your computer) and listening on 50051. Remember how we set the port to 50051 above when starting mavsdk_server -p 50051? This is the reason why. This also means that you could run MAVSDK-Java on one computer, and mavsdk_server on another, as long as you set the IP:port correctly!

Now that we have a drone, we can start using it. Let’s arm:

jshell> drone.getAction().arm().subscribe()

If you look at QGC now, you should see that the drone is “Armed” in the top toolbar. Now, what is this .subscribe()? That’s just how RxJava works, and I highly recommend reading a bit about it; MAVSDK-Java exposes everything through RxJava constructs. In short, MAVSDK-Java will provide RxJava objects like Completable or Flowable, and those get executed only when you subscribe. In the example above, drone.getAction().arm() therefore gives you a Completable, as can be verified from the REPL (actually it says “CompletableCreate”, but let’s just take that as a Completable):

jshell> drone.getAction().arm()
$4 ==> io.reactivex.internal.operators.completable.CompletableCreate@76f4b65

An interesting concept about Completable is that they can be combined to create a new Completable. Say that we want to take off. Before taking off, we need to arm. Why not creating a new Completable that arms first, and then takes off if the arming succeeded? Let’s just do it:

jshell> import io.reactivex.Completable
jshell> Completable armAndTakeoff = drone.getAction().arm().andThen(drone.getAction().takeoff())
armAndTakeoff ==> io.reactivex.internal.operators.completable.CompletableConcatArray@77102b91

Again, we see that this returns a Completable. If you look at QGC, the drone did not takeoff. Because we did not subscribe to it. Let’s do it:

jshell> armAndTakeoff.subscribe()

Now check in QGC that the drone did take off this time.

QGroundControl showing that the drone took off

QGroundControl showing that the drone took off.

RxJava gets very powerful when we start dealing with streams (which are represented by the Flowable and Observable types). As an example, let’s get the altitude of the drone. The altitude is encoded in the Telemetry.Position object, in the Telemetry plugin. When subscribing to a Flowable, we can pass an “onNext” function that gets executed for each new event, i.e. each time a new position is advertised by the drone. Let’s print 3 position events in the REPL:

jshell> drone.getTelemetry().getPosition().take(3).subscribe(position -> java.lang.System.out.println(position))
$3 ==> true

io.mavsdk.telemetry.Telemetry$Position@65e6d53b
io.mavsdk.telemetry.Telemetry$Position@3469e1c1
io.mavsdk.telemetry.Telemetry$Position@23ed2234

We see here that the REPL printed 3 Position objects (though in a not super readable way, I admit). Two things require attention here. First, the .take(3) made it a Flowable of maximum 3 elements. Without this modifier, the stream will be infinite: you will get Position events as long as the drone is connected. It is just a bit annoying to indefinitely spam the terminal with those events, so I made it a finite stream of 3 elements (if you don’t get what I mean, just try without .take(3) and you will quickly understand =)). The second thing is that I called java.lang.System.out.println. That’s because System refers to io.mavsdk.System. It is arguably an unfortunate naming, but I won’t go in that discussion here.

I mentioned earlier that RxJava was super powerful with streams, let me show why. We said above that we were interested in getting the drone altitude. But here we get the position. We can transform this Flowable into a Flowable representing the altitude with the map() operator:

Flowable altitudeFlowable = drone.getTelemetry().getPosition().map(position -> position.getRelativeAltitudeM())

And now we have a new Flowable to which we can subscribe to get altitude events:

altitudeFlowable.take(3).subscribe(altitude -> java.lang.System.out.println(altitude))

Now let’s say (for the sake of the example) that we want something more complicated: we want to round the altitude (because we don’t care about the decimals). Also, we want to receive an event only when the altitude changes (as opposed to receiving events telling me that the altitude is the same as what the previous event was saying). And finally we are only interested in getting events when the drone is close to the ground, say lower than 5m above ground. We can create a new Flowable just for that:

Flowable mySpecialAltitudeFlowable = drone.getTelemetry()
                                                   .getPosition()
                                                   .map(position -> Math.round(position.getRelativeAltitudeM()))
                                                   .distinctUntilChanged()
                                                   .filter(altitude -> altitude <= 5)

If you’re not convinced that this is powerful, just stop for a minute and try to imagine how you would accomplish the same if MAVSDK was exposing the position through an old-school callback…

Now that we explored the basics of RxJava, the next step is probably to have a look at the Java examples we provide in the repository! When writing your own application, just note that MAVSDK-Java is available on Maven Central, and therefore added as a gradle dependency with:

implementation 'io.mavsdk:mavsdk:0.2.0'

Getting out of RxJava

MAVSDK exposes RxJava not only because it is powerful, but also because it is easy to get a blocking interface out of it. In case you are not comfortable with reactive programming, it is extremely easy to get out of RxJava thanks to the blocking operators. You want a blocking, old-school arm() function? Here it is:

public void arm() {
    drone.getAction().arm().blockingAwait();
}

You want a way to poll for the position? No worries:

public Telemetry.Position getPosition() {
    drone.getTelemetry().getPosition().blockingFirst();
}

It is really just a matter of checking the functions offered by the autocomplete and look for “blocking” operators. RxJava is extremely well documented, meaning that there is absolutely no challenge in doing that.

Taking off in Android

We have seen above that MAVSDK-Java connects to mavsdk_server, which in turn connects to the drone. It is working exactly the same on Android. The only question is whether mavsdk_server runs onboard the Android phone or not.

Say we keep the setup described above, with mavsdk_server running on your computer. If your Android phone is on the same network as the computer, then you can directly connect to it with:

System drone = new System(IP_OF_YOUR_COMPUTER, 50051);

Now usually, in production setup, you will want to run mavsdk_server on the smartphone. This is already possible for arm64-v8a, and it will come soon with armeabi-v7a as well. The android sample app is doing it. In that setup, mavsdk_server comes as an Android library together with MAVSDK-Java (see here):

implementation 'io.mavsdk:mavsdk:0.2.0'
implementation 'io.mavsdk:mavsdk-server:0.2.0'

All you need is then to start mavsdk_server before you start using your System object (which we called drone in this document), as shown in the Android sample app:

new Thread(() -> {
    MavsdkServer mavsdkServer = new MavsdkServer();
    mavsdkServer.run("udp://:14540", 50020);
}).start();

Note that this will slightly change in the future, but in a very obvious way: instead of starting a thread, it will be a matter of running mavsdkServer.start().subscribe(), or something like that. All the rest is just Java!

2020-03-26T07:27:19+00:00