Improve integration tests, add integration tests to CI

Jenkinsfile

@@ -119,6 +119,33 @@ pipeline {
           }
         }
 
+        stage('Ubuntu 18.04 Debug PX4 SITL') {
+          agent {
+            docker {
+              image 'dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl:2019-05-15'
+              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+            }
+          }
+          steps {
+            sh 'export'
+            sh 'git submodule deinit -f .'
+            sh 'git clean -ff -x -d .'
+            sh 'git submodule sync --recursive'
+            sh 'git submodule update --init --recursive --force'
+            sh 'ccache -z'
+            sh 'make BUILD_TYPE=Debug'
+            sh 'cp -r /home/user/Firmware `pwd`/Firmware'
+            sh 'HOME=`pwd` PX4_SIM_SPEED_FACTOR=10 AUTOSTART_SITL=1 PX4_FIRMWARE_DIR=`pwd`/Firmware HEADLESS=1 build/default/integration_tests/integration_tests_runner --gtest_filter="SitlTest.*"'
+          }
+          post {
+            always {
+              sh 'ccache -s'
+              sh 'git submodule deinit -f .'
+              sh 'git clean -ff -x -d .'
+            }
+          }
+        }
+
         stage('Fedora 27 Debug') {
           agent {
             docker {

cmake/compiler_flags.cmake

@@ -47,9 +47,6 @@ if(UNIX AND NOT APPLE)
     add_definitions("-DLINUX")
 endif()
 
-# Add DEBUG define for Debug target because that is not done automatically.
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -DDEBUG")
-
 set(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_COVERAGE} --coverage")
 set(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_COVERAGE} --coverage")
 set(CMAKE_LINKER_FLAGS_COVERAGE "${CMAKE_LINKER_FLAGS_COVERAGE} --coverage")

core/log.h

@@ -17,23 +17,7 @@
 #define __FILENAME__ __FILE__
 #endif
 
-// For release builds, don't show debug printfs, and just discard it into the NullStream.
-class NullStream {
-public:
-    template<typename TPrintable> NullStream &operator<<(TPrintable const &)
-    {
-        /* no-op */
-        static NullStream nothing;
-        return nothing;
-    }
-};
-
-#if DEBUG
 #define LogDebug() LogDebugDetailed(__FILENAME__, __LINE__)
-#else
-#define LogDebug() NullStream()
-#endif
-
 #define LogInfo() LogInfoDetailed(__FILENAME__, __LINE__)
 #define LogWarn() LogWarnDetailed(__FILENAME__, __LINE__)
 #define LogErr() LogErrDetailed(__FILENAME__, __LINE__)
@@ -122,7 +106,7 @@ class LogDetailed {
         set_color(Color::RESET);
 
         std::cout << _s.str();
-        std::cout << " (" << _caller_filename << ":" << _caller_filenumber << ")";
+        std::cout << " (" << _caller_filename << ":" << std::dec << _caller_filenumber << ")";
 
         std::cout << std::endl;
 #endif

docker/Dockerfile-Ubuntu-18.04-PX4-SITL

@@ -0,0 +1,35 @@
+#
+# PX4 SITL testing environment for the Dronecode SDK based on Ubuntu 18.04.
+#
+# Author: Julian Oes <julian@oes.ch>
+#
+FROM dronecode/dronecode-sdk-ubuntu-18.04
+MAINTAINER Julian Oes <julian@oes.ch>
+
+ENV FIRMWARE_DIR ${WORKDIR}../Firmware
+
+RUN apt-get update && \
+    apt-get install -y cmake \
+                       curl \
+                       git \
+                       libeigen3-dev \
+                       libopencv-dev \
+                       libroscpp-dev \
+                       protobuf-compiler \
+                       python-empy \
+                       python-jinja2 \
+                       python-numpy \
+                       python-toml \
+                       python-yaml \
+                       unzip \
+                       gazebo9 \
+                       libgazebo9-dev \
+                       ninja-build \
+                       psmisc \
+    && apt-get -y autoremove \
+    && apt-get clean autoclean \
+    && rm -rf /var/lib/apt/lists/{apt,dpkg,cache,log} /tmp/* /var/tmp/*
+
+RUN git clone https://github.com/PX4/Firmware.git ${FIRMWARE_DIR}
+RUN git -C ${FIRMWARE_DIR} submodule update --init --recursive
+RUN cd ${FIRMWARE_DIR} && DONT_RUN=1 make px4_sitl gazebo && DONT_RUN=1 make px4_sitl gazebo

docker/build_and_push_docker_images.sh

@@ -6,18 +6,21 @@ docker pull dronecode/dronecode-sdk-fedora-28
 docker pull dronecode/dronecode-sdk-fedora-29
 docker pull dronecode/dronecode-sdk-ubuntu-16.04
 docker pull dronecode/dronecode-sdk-ubuntu-18.04
+docker pull dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl
 
 docker build -f Dockerfile-Fedora-28 -t dronecode/dronecode-sdk-fedora-28 .
 docker build -f Dockerfile-Fedora-29 -t dronecode/dronecode-sdk-fedora-29 .
 docker build -f Dockerfile-Ubuntu-16.04 -t dronecode/dronecode-sdk-ubuntu-16.04 .
 docker build -f Dockerfile-Ubuntu-18.04 -t dronecode/dronecode-sdk-ubuntu-18.04 .
+docker build -f Dockerfile-Ubuntu-18.04-PX4-SITL -t dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl .
 
 DATE=`date +%Y-%m-%d`
 
 docker tag dronecode/dronecode-sdk-fedora-28:latest dronecode/dronecode-sdk-fedora-28:$DATE
 docker tag dronecode/dronecode-sdk-fedora-29:latest dronecode/dronecode-sdk-fedora-29:$DATE
 docker tag dronecode/dronecode-sdk-ubuntu-16.04:latest dronecode/dronecode-sdk-ubuntu-16.04:$DATE
 docker tag dronecode/dronecode-sdk-ubuntu-18.04:latest dronecode/dronecode-sdk-ubuntu-18.04:$DATE
+docker tag dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl:latest dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl:$DATE
 
 docker push dronecode/dronecode-sdk-fedora-28:latest
 docker push dronecode/dronecode-sdk-fedora-28:$DATE
@@ -27,5 +30,7 @@ docker push dronecode/dronecode-sdk-ubuntu-16.04:latest
 docker push dronecode/dronecode-sdk-ubuntu-16.04:$DATE
 docker push dronecode/dronecode-sdk-ubuntu-18.04:latest
 docker push dronecode/dronecode-sdk-ubuntu-18.04:$DATE
+docker push dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl:latest
+docker push dronecode/dronecode-sdk-ubuntu-18.04-px4-sitl:$DATE
 
 echo "Please update tag in Jenkinsfile to '$DATE'"

integration_tests/CMakeLists.txt

@@ -46,7 +46,6 @@ add_executable(integration_tests_runner
     mission_change_speed.cpp
     mission.cpp
     mission_rtl.cpp
-    mission_survey.cpp
     mission_raw_mission_changed.cpp
     offboard_velocity.cpp
     offboard_position.cpp

integration_tests/action_goto.cpp

@@ -15,8 +15,8 @@ TEST_F(SitlTest, ActionGoto)
     ASSERT_EQ(ret, ConnectionResult::SUCCESS);
 
     // Wait for system to connect via heartbeat.
-    std::this_thread::sleep_for(std::chrono::seconds(2));
-    ASSERT_TRUE(dc.is_connected());
+    ASSERT_TRUE(poll_condition_with_timeout([&dc]() { return dc.is_connected(); },
+                                            std::chrono::seconds(10)));
 
     System &system = dc.system();
     auto telemetry = std::make_shared<Telemetry>(system);
@@ -26,7 +26,7 @@ TEST_F(SitlTest, ActionGoto)
         LogInfo() << "waiting for system to be ready";
         std::this_thread::sleep_for(std::chrono::seconds(1));
 
-        ASSERT_LT(++iteration, 20);
+        ASSERT_LT(++iteration, 10);
     }
 
     auto action = std::make_shared<Action>(system);
@@ -55,7 +55,7 @@ TEST_F(SitlTest, ActionGoto)
         std::this_thread::sleep_for(std::chrono::seconds(1));
 
         // TODO: currently we need to wait a long time until landed is detected.
-        ASSERT_LT(++iteration, 10);
+        ASSERT_LT(++iteration, 20);
     }
 
     action_ret = action->disarm();

integration_tests/action_hover_async.cpp

@@ -1,19 +1,12 @@
-#include <iostream>
 #include "integration_test_helper.h"
 #include "global_include.h"
 #include "dronecode_sdk.h"
 #include "plugins/action/action.h"
 #include "plugins/telemetry/telemetry.h"
 
-using namespace dronecode_sdk;
-using namespace std::placeholders; // for `_1`
-
-void receive_result(Action::Result result);
-void receive_health_all_ok(bool all_ok);
-void receive_in_air(bool in_air);
+#include <future>
 
-static bool _all_ok = false;
-static bool _in_air = false;
+using namespace dronecode_sdk;
 
 TEST_F(SitlTest, ActionHoverAsync)
 {
@@ -22,55 +15,96 @@ TEST_F(SitlTest, ActionHoverAsync)
     ConnectionResult ret = dc.add_udp_connection();
     ASSERT_EQ(ret, ConnectionResult::SUCCESS);
 
-    // Wait for system to connect via heartbeat.
-    std::this_thread::sleep_for(std::chrono::seconds(2));
+    {
+        LogInfo() << "Waiting to discover vehicle";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        dc.register_on_discover([&prom](uint64_t uuid) {
+            prom.set_value();
+            UNUSED(uuid);
+        });
+        ASSERT_EQ(fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);
+    }
 
-    // TODO: this test is pretty dumb, should be improved with more checks.
     System &system = dc.system();
-
     auto telemetry = std::make_shared<Telemetry>(system);
-    telemetry->health_all_ok_async(std::bind(&receive_health_all_ok, _1));
-    telemetry->in_air_async(std::bind(&receive_in_air, _1));
+    auto action = std::make_shared<Action>(system);
 
-    while (!_all_ok) {
-        std::cout << "Waiting to be ready..." << std::endl;
-        std::this_thread::sleep_for(std::chrono::seconds(1));
+    {
+        LogDebug() << "Waiting to be ready...";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        telemetry->health_all_ok_async([&telemetry, &prom](bool all_ok) {
+            if (all_ok) {
+                // Unregister to prevent fulfilling promise twice.
+                telemetry->health_all_ok_async(nullptr);
+                prom.set_value();
+            }
+        });
+        ASSERT_EQ(fut.wait_for(std::chrono::seconds(10)), std::future_status::ready);
     }
 
-    auto action = std::make_shared<Action>(system);
-    action->arm_async(std::bind(&receive_result, _1));
-    std::this_thread::sleep_for(std::chrono::seconds(2));
+    {
+        LogInfo() << "Arming";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        action->arm_async([&prom](Action::Result result) {
+            EXPECT_EQ(result, Action::Result::SUCCESS);
+            prom.set_value();
+        });
+        EXPECT_EQ(fut.wait_for(std::chrono::seconds(2)), std::future_status::ready);
+    }
 
+    LogInfo() << "Setting takeoff altitude";
     action->set_takeoff_altitude(5.0f);
 
-    action->takeoff_async(std::bind(&receive_result, _1));
-    std::this_thread::sleep_for(std::chrono::seconds(5));
-
-    action->land_async(std::bind(&receive_result, _1));
-
-    while (_in_air) {
-        std::cout << "Waiting to be landed..." << std::endl;
-        std::this_thread::sleep_for(std::chrono::seconds(1));
+    {
+        LogInfo() << "Taking off";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        action->takeoff_async([&prom](Action::Result result) {
+            EXPECT_EQ(result, Action::Result::SUCCESS);
+            prom.set_value();
+        });
+        EXPECT_EQ(fut.wait_for(std::chrono::seconds(2)), std::future_status::ready);
     }
 
-    action->disarm_async(std::bind(&receive_result, _1));
-    std::this_thread::sleep_for(std::chrono::seconds(2));
-}
+    // TODO: adapt this time based on altitude
+    std::this_thread::sleep_for(std::chrono::seconds(5));
 
-void receive_result(Action::Result result)
-{
-    EXPECT_EQ(result, Action::Result::SUCCESS);
-}
+    {
+        LogInfo() << "Landing";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        action->land_async([&prom](Action::Result result) {
+            EXPECT_EQ(result, Action::Result::SUCCESS);
+            prom.set_value();
+        });
+        EXPECT_EQ(fut.wait_for(std::chrono::seconds(2)), std::future_status::ready);
+    }
 
-void receive_health_all_ok(bool all_ok)
-{
-    if (all_ok && !_all_ok) {
-        LogDebug() << "we're ready, let's go";
-        _all_ok = true;
+    {
+        LogInfo() << "Waiting to be landed...";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        telemetry->in_air_async([&telemetry, &prom](bool in_air) {
+            if (!in_air) {
+                // Unregister to prevent fulfilling promise twice.
+                telemetry->in_air_async(nullptr);
+                prom.set_value();
+            }
+        });
+        EXPECT_EQ(fut.wait_for(std::chrono::seconds(20)), std::future_status::ready);
     }
-}
 
-void receive_in_air(bool in_air)
-{
-    _in_air = in_air;
+    {
+        LogInfo() << "Disarming";
+        std::promise<void> prom;
+        std::future<void> fut = prom.get_future();
+        action->disarm_async([&prom](Action::Result result) {
+            EXPECT_EQ(result, Action::Result::SUCCESS);
+            prom.set_value();
+        });
+        EXPECT_EQ(fut.wait_for(std::chrono::seconds(2)), std::future_status::ready);
+    }
 }

integration_tests/action_hover_sync.cpp

@@ -32,24 +32,20 @@ void takeoff_and_hover_at_altitude(float altitude_m)
     ASSERT_EQ(ret, ConnectionResult::SUCCESS);
 
     // Wait for system to connect via heartbeat.
-    std::this_thread::sleep_for(std::chrono::seconds(2));
-    ASSERT_TRUE(dc.is_connected());
+    LogInfo() << "Waiting for system connect";
+    ASSERT_TRUE(poll_condition_with_timeout([&dc]() { return dc.is_connected(); },
+                                            std::chrono::seconds(10)));
 
     System &system = dc.system();
     auto telemetry = std::make_shared<Telemetry>(system);
 
-    int iteration = 0;
-    while (!telemetry->health_all_ok()) {
-        LogInfo() << "waiting for system to be ready";
-        std::this_thread::sleep_for(std::chrono::seconds(1));
-
-        ASSERT_LT(++iteration, 10);
-    }
+    LogInfo() << "Waiting for system to be ready";
+    ASSERT_TRUE(poll_condition_with_timeout([&telemetry]() { return telemetry->health_all_ok(); },
+                                            std::chrono::seconds(10)));
 
     auto action = std::make_shared<Action>(system);
     Action::Result action_ret = action->arm();
     EXPECT_EQ(action_ret, Action::Result::SUCCESS);
-    std::this_thread::sleep_for(std::chrono::seconds(1));
 
     EXPECT_EQ(Action::Result::SUCCESS, action->set_takeoff_altitude(altitude_m));
     auto takeoff_altitude_result = action->get_takeoff_altitude();
@@ -58,8 +54,12 @@ void takeoff_and_hover_at_altitude(float altitude_m)
 
     action_ret = action->takeoff();
     EXPECT_EQ(action_ret, Action::Result::SUCCESS);
-    // We wait 1.5s / m plus a margin of 3s.
-    const int wait_time_s = static_cast<int>(altitude_m * 1.5f + 3.0f);
+
+    // TODO: The wait time should not be hard-coded because the
+    //       simulation might run faster.
+
+    // We wait 2s / m plus a margin of 5s.
+    const int wait_time_s = static_cast<int>(altitude_m * 2.0f + 5.0f);
     std::this_thread::sleep_for(std::chrono::seconds(wait_time_s));
 
     EXPECT_GT(telemetry->position().relative_altitude_m, altitude_m - 0.25f);
@@ -68,14 +68,8 @@ void takeoff_and_hover_at_altitude(float altitude_m)
     action_ret = action->land();
     EXPECT_EQ(action_ret, Action::Result::SUCCESS);
 
-    iteration = 0;
-    while (telemetry->in_air()) {
-        LogInfo() << "waiting for system to be landed";
-        std::this_thread::sleep_for(std::chrono::seconds(1));
-
-        // TODO: currently we need to wait a long time until landed is detected.
-        ASSERT_LT(++iteration, 2 * wait_time_s);
-    }
+    EXPECT_TRUE(poll_condition_with_timeout([&telemetry]() { return !telemetry->in_air(); },
+                                            std::chrono::seconds(wait_time_s)));
 
     action_ret = action->disarm();
     EXPECT_EQ(action_ret, Action::Result::SUCCESS);