Integration tests (#52)

Signed-off-by: Arjo Chakravarty <[email protected]> Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Arjo Chakravarty <[email protected]>
osrf · Nov 2, 2021 · 9ab9015 · 9ab9015
1 parent 3cee923
commit 9ab9015
Show file tree

Hide file tree

Showing 11 changed files with 399 additions and 11 deletions.
diff --git a/.github/workflows/integration_tests.yaml b/.github/workflows/integration_tests.yaml
@@ -0,0 +1,19 @@
+name: integration_tests
+
+on: [push, pull_request]
+
+jobs:
+  integration_tests:
+    runs-on: ubuntu-20.04 # linux required if you want to use docker
+    steps:
+    - name: Remove android, dotnet and other bull$#!@
+      run: |
+        sudo rm -rf /usr/local/lib/android
+        sudo rm -rf /usr/share/dotnet
+        sudo rm -rf /opt/ghc
+    - name: Check out code
+      uses: actions/checkout@v2
+    - name: Build docker images
+      run: docker build -t mbari_lrauv . -f docker/tests/Dockerfile
+    - name: Run tests
+      run: docker run mbari_lrauv
diff --git a/docker/empty_world/Dockerfile b/docker/empty_world/Dockerfile
@@ -99,6 +99,8 @@ RUN [ "/bin/bash" , "-c" , \
   && colcon build \
   && apt-get -qq clean" ]
 
+SHELL ["/bin/bash", "-c"]
+
 # start ignition
 ENTRYPOINT [ "/bin/bash" , "-c" , \
   "source /home/colcon_ws/install/setup.bash \

diff --git a/docker/tests/Dockerfile b/docker/tests/Dockerfile
@@ -0,0 +1,61 @@
+#
+# Copyright (C) 2021 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+FROM ubuntu:20.04
+
+# This avoids keyboard interaction when asked for geographic area
+ARG DEBIAN_FRONTEND=noninteractive
+
+# setup timezone
+RUN echo 'Etc/UTC' > /etc/timezone && \
+  ln -fs /usr/share/zoneinfo/Etc/UTC /etc/localtime
+
+RUN apt-get update \
+ && apt-get install -y \
+        build-essential \
+        curl \
+        gnupg2 \
+        lsb-release \
+        tzdata \
+        wget
+
+# Add ROS packages for colcon
+RUN /bin/sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' \
+ && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add -
+
+# Add Ignition nightly packages
+RUN /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
+    /bin/sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' && \
+    /bin/sh -c 'wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -'
+
+# Install colcon and Ignition
+RUN apt-get -qq update && apt-get -q -y install \
+  python3-colcon-common-extensions \
+  ignition-fortress
+
+# Clean up apt
+RUN rm -rf /var/lib/apt/lists/* \
+  && apt-get -qq clean
+
+# Build LRAUV
+RUN mkdir -p /home/colcon_ws/src
+COPY . /home/colcon_ws/src
+
+WORKDIR /home/colcon_ws
+RUN [ "/bin/bash" , "-c" , "colcon build --cmake-args='-DBUILD_TESTING=true'" ]
+
+# Run tests
+ENTRYPOINT [ "/bin/bash" , "-c" , "colcon test" ]
diff --git a/lrauv_ignition_plugins/CMakeLists.txt b/lrauv_ignition_plugins/CMakeLists.txt
@@ -4,7 +4,6 @@ project(lrauv_ignition_plugins)
 
 #============================================================================
 # Find dependencies
-
 find_package(ignition-cmake2 REQUIRED)
 
 find_package(ignition-gazebo6 REQUIRED)
@@ -118,6 +117,51 @@ configure_file(
   "${CMAKE_CURRENT_BINARY_DIR}/hooks/hook.dsv" @ONLY
 )
 
+#============================================================================
+# Tests
+if(BUILD_TESTING)
+
+  # Fetch and configure GTest
+  include(FetchContent)
+  FetchContent_Declare(
+    googletest
+    URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
+  )
+  set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+  FetchContent_MakeAvailable(googletest)
+
+  enable_testing()
+  include(Dart)
+
+  # Build-time constants
+  set("PROJECT_BINARY_PATH" ${CMAKE_CURRENT_BINARY_DIR})
+  set("PROJECT_SOURCE_PATH" ${CMAKE_CURRENT_SOURCE_DIR})
+  configure_file(test/helper/TestConstants.hh.in TestConstants.hh @ONLY)
+
+  # Tests
+  foreach(TEST_TARGET
+    test_controller
+    test_drop_weight
+    test_mass_shifter)
+
+    add_executable(
+      ${TEST_TARGET}
+      test/${TEST_TARGET}.cc
+    )
+    target_include_directories(${TEST_TARGET}
+      PUBLIC ${CMAKE_BINARY_DIR}/proto)
+    target_link_libraries(${TEST_TARGET}
+      PUBLIC gtest_main
+      PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
+      lrauv_command
+    )
+    include_directories(${CMAKE_CURRENT_BINARY_DIR})
+    include(GoogleTest)
+    gtest_discover_tests(${TEST_TARGET})
+  endforeach()
+
+endif()
+
 #============================================================================
 # Resources
 

diff --git a/lrauv_ignition_plugins/src/JointPositionPlugin.cc b/lrauv_ignition_plugins/src/JointPositionPlugin.cc
@@ -99,7 +99,7 @@ void TethysJointPlugin::Configure(
     this->dataPtr->jointIndex = _sdf->Get<unsigned int>("joint_index");
   }
 
-  std::string topic = 
+  std::string topic =
     ignition::transport::TopicUtils::AsValidTopic("/model/" +
     this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
     "/" + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos");
@@ -176,7 +176,7 @@ void TethysJointPlugin::PreUpdate(
 
   std::chrono::duration<double, std::ratio<1,1>> deltaT = _info.dt;
   auto maxPositionChange = deltaT.count() * this->dataPtr->maxVelocity;
-  
+
   double desiredVelocity = 0;
   if (cmdPos < currentPos[this->dataPtr->jointIndex])
   {

diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc
@@ -346,14 +346,7 @@ void TethysCommPlugin::CommandCallback(
   //    > std::chrono::milliseconds(1000))
   {
     igndbg << "Received command: " << std::endl
-      << "  propOmegaAction_: " << _msg.propomegaaction_() << std::endl
-      << "  rudderAngleAction_: " << _msg.rudderangleaction_() << std::endl
-      << "  elevatorAngleAction_: " << _msg.elevatorangleaction_() << std::endl
-      << "  massPositionAction_: " << _msg.masspositionaction_() << std::endl
-      << "  buoyancyAction_: " << _msg.buoyancyaction_() << std::endl
-      << "  density_: " << _msg.density_() << std::endl
-      << "  dt_: " << _msg.dt_() << std::endl
-      << "  time_: " << _msg.time_() << std::endl;
+      << _msg.DebugString() << std::endl;
 
     this->prevSubPrintTime = std::chrono::seconds(int(floor(_msg.time_())));
   }

diff --git a/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh b/lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
@@ -0,0 +1,105 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef LRAUVIGNITIONPLUGINS_LRAUVTESTFIXTURE_HH_
+#define LRAUVIGNITIONPLUGINS_LRAUVTESTFIXTURE_HH_
+
+#include <gtest/gtest.h>
+
+#include <ignition/gazebo/TestFixture.hh>
+#include <ignition/gazebo/Util.hh>
+#include <ignition/gazebo/World.hh>
+#include <ignition/transport/Node.hh>
+
+#include "lrauv_command.pb.h"
+
+#include "TestConstants.hh"
+
+using namespace std::chrono_literals;
+
+/// \brief Convenient fixture that provides boilerplate code common to most
+/// LRAUV tests.
+class LrauvTestFixture : public ::testing::Test
+{
+  // Documentation inherited
+  protected: void SetUp() override
+  {
+    ignition::common::Console::SetVerbosity(4);
+
+    // Setup transport
+    auto commandTopic = "/tethys/command_topic";
+    this->commandPub =
+      this->node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(
+      commandTopic);
+
+    // Setup fixture
+    this->fixture = std::make_unique<ignition::gazebo::TestFixture>(
+        ignition::common::joinPaths(
+        std::string(PROJECT_SOURCE_PATH), "worlds", "buoyant_tethys.sdf"));
+
+    fixture->OnPostUpdate(
+      [&](const ignition::gazebo::UpdateInfo &_info,
+      const ignition::gazebo::EntityComponentManager &_ecm)
+      {
+        auto worldEntity = ignition::gazebo::worldEntity(_ecm);
+        ignition::gazebo::World world(worldEntity);
+
+        auto modelEntity = world.ModelByName(_ecm, "tethys");
+        EXPECT_NE(ignition::gazebo::kNullEntity, modelEntity);
+
+        this->tethysPoses.push_back(
+            ignition::gazebo::worldPose(modelEntity, _ecm));
+        this->iterations++;
+      });
+    fixture->Finalize();
+  }
+
+  /// \brief Keep publishing a command while a condition is met or it times out
+  /// \param[in] _msg Command to be published
+  /// \param[in] _continue Function that returns true if we should keep
+  /// publishing, that is, something that is initially true but should become
+  /// false as the command is processed.
+  public: void PublishCommandWhile(
+      const lrauv_ignition_plugins::msgs::LRAUVCommand &_msg,
+      std::function<bool()> _continue)
+  {
+    int maxSleep{30};
+    int sleep{0};
+    for (; sleep < maxSleep && _continue(); ++sleep)
+    {
+      this->commandPub.Publish(_msg);
+      this->fixture->Server()->Run(true, 300, false);
+      std::this_thread::sleep_for(100ms);
+    }
+    EXPECT_LT(sleep, maxSleep);
+  }
+
+  /// \brief How many times has OnPostUpdate been run
+  public: unsigned int iterations{0u};
+
+  /// \brief All tethys world poses in order
+  public: std::vector<ignition::math::Pose3d> tethysPoses;
+
+  /// \brief Test fixture
+  public: std::unique_ptr<ignition::gazebo::TestFixture> fixture{nullptr};
+
+  /// \brief Node for communication
+  public: ignition::transport::Node node;
+
+  /// \brief Publishes commands
+  public: ignition::transport::Node::Publisher commandPub;
+};
+#endif
diff --git a/lrauv_ignition_plugins/test/helper/TestConstants.hh.in b/lrauv_ignition_plugins/test/helper/TestConstants.hh.in
@@ -0,0 +1,5 @@
+#ifndef __TEST_CONSTANTS_HH__
+#define __TEST_CONSTANTS_HH__
+#cmakedefine PROJECT_BINARY_PATH "@PROJECT_BINARY_PATH@"
+#cmakedefine PROJECT_SOURCE_PATH "@PROJECT_SOURCE_PATH@"
+#endif
diff --git a/lrauv_ignition_plugins/test/test_controller.cc b/lrauv_ignition_plugins/test/test_controller.cc
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <chrono>
+#include <gtest/gtest.h>
+
+#include "helper/LrauvTestFixture.hh"
+#include "lrauv_command.pb.h"
+
+#include <fstream>
+
+//////////////////////////////////////////////////
+TEST_F(LrauvTestFixture, Command)
+{
+  // Get initial X
+  this->fixture->Server()->Run(true, 100, false);
+  EXPECT_EQ(100, this->iterations);
+  EXPECT_EQ(100, this->tethysPoses.size());
+  EXPECT_NEAR(0.0, this->tethysPoses.back().Pos().X(), 1e-6);
+
+  // Propel vehicle
+  lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
+  cmdMsg.set_propomegaaction_(30);
+
+  // Neutral buoyancy
+  cmdMsg.set_buoyancyaction_(500);
+
+  // Run server until the command is processed and the model reaches a certain
+  // point ahead (the robot moves towards -X)
+  double targetX{-1.5};
+  this->PublishCommandWhile(cmdMsg, [&]()
+  {
+    return this->tethysPoses.back().Pos().X() > targetX;
+  });
+
+  EXPECT_LT(100, this->iterations);
+  EXPECT_LT(100, this->tethysPoses.size());
+  EXPECT_GT(targetX, this->tethysPoses.back().Pos().X());
+}
+