adding unit tests

CMakeLists.txt

@@ -157,6 +157,16 @@ target_link_libraries(MrsUavManagers_TransformManager
   ${Eigen_LIBRARIES}
   )
 
+## --------------------------------------------------------------
+## |                           Testing                          |
+## --------------------------------------------------------------
+
+if(CATKIN_ENABLE_TESTING)
+
+  add_subdirectory(test)
+
+endif()
+
 ## --------------------------------------------------------------
 ## |                           Install                          |
 ## --------------------------------------------------------------

launch/estimation_manager.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- args corresponding to environment variables -->
-  <arg name="UAV_NAME" default="$(env UAV_NAME)" />
+  <arg name="UAV_NAME" default="$(optenv UAV_NAME)" />
   <arg name="PROFILER" default="$(optenv PROFILER false)" />
   <arg name="RUN_TYPE" default="$(optenv RUN_TYPE)" />
 

launch/transform_manager.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- args corresponding to environment variables -->
-  <arg name="UAV_NAME" default="$(env UAV_NAME)" />
+  <arg name="UAV_NAME" default="$(optenv UAV_NAME)" />
   <arg name="PROFILER" default="$(optenv PROFILER false)" />
   <arg name="RUN_TYPE" default="$(optenv RUN_TYPE)" />
 

package.xml

@@ -25,6 +25,7 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>rostest</depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelets.xml" />

test/CMakeLists.txt

@@ -0,0 +1,37 @@
+find_package(rostest REQUIRED)
+
+# takeoff test
+
+add_rostest_gtest(MrsUavManagers_UavManagerTakeoffTest
+  uav_manager/takeoff/uav_manager_takeoff.test
+  uav_manager/takeoff/test.cpp
+  )
+
+target_link_libraries(
+  MrsUavManagers_UavManagerTakeoffTest
+  ${catkin_LIBRARIES}
+  )
+
+add_dependencies(
+  MrsUavManagers_UavManagerTakeoffTest
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  )
+
+# goto test
+
+add_rostest_gtest(MrsUavManagers_ControlManagerGotoTest
+  control_manager/goto/control_manager_goto.test
+  control_manager/goto/test.cpp
+  )
+
+target_link_libraries(
+  MrsUavManagers_ControlManagerGotoTest
+  ${catkin_LIBRARIES}
+  )
+
+add_dependencies(
+  MrsUavManagers_ControlManagerGotoTest
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  )

test/control_manager/goto/control_manager_goto.test

@@ -0,0 +1,9 @@
+<launch>
+
+  <env name="ROSCONSOLE_CONFIG_FILE" value="$(find mrs_uav_managers)/config/debug_verbosity.yaml" />
+
+  <include file="$(find mrs_uav_managers)/test/sessions/x500_in_mid_air/launch/mrs_uav_system.launch" />
+
+  <test pkg="mrs_uav_managers" type="MrsUavManagers_ControlManagerGotoTest" test-name="goto_test" time-limit="60.0" />
+
+</launch>

test/control_manager/goto/test.cpp

@@ -0,0 +1,158 @@
+#include <ros/ros.h>
+#include <ros/package.h>
+
+#include <mrs_lib/subscribe_handler.h>
+#include <mrs_lib/service_client_handler.h>
+#include <mrs_lib/attitude_converter.h>
+
+#include <std_msgs/Bool.h>
+#include <mrs_msgs/ControlManagerDiagnostics.h>
+#include <mrs_msgs/EstimationDiagnostics.h>
+#include <mrs_msgs/Vec4.h>
+
+#include <std_srvs/SetBool.h>
+#include <std_srvs/Trigger.h>
+
+#include <gtest/gtest.h>
+#include <ros/console.h>
+#include <log4cxx/logger.h>
+
+/* TEST(TESTSuite, goto) //{ */
+
+TEST(TESTSuite, goto) {
+
+  int result = 0;
+
+  // | ------------------ initialize test node ------------------ |
+
+  ros::NodeHandle nh = ros::NodeHandle("~");
+
+  ROS_INFO("[%s]: ROS node initialized", ros::this_node::getName().c_str());
+
+  ros::Time::waitForValid();
+
+  std::string uav_name = "uav1";
+
+  // | ----------------------- subscribers ---------------------- |
+
+  mrs_lib::SubscribeHandlerOptions shopts;
+  shopts.nh                 = nh;
+  shopts.node_name          = "Test";
+  shopts.no_message_timeout = mrs_lib::no_timeout;
+  shopts.threadsafe         = true;
+  shopts.autostart          = true;
+  shopts.queue_size         = 10;
+  shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+
+  mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics> sh_control_manager_diag =
+      mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts, "/" + uav_name + "/control_manager/diagnostics");
+
+  mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics> sh_estim_manager_diag =
+      mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts, "/" + uav_name + "/estimation_manager/diagnostics");
+
+  std::cout << "[Test]: subscribers initialized" << std::endl;
+
+  // | --------------------- service clients -------------------- |
+
+  mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh, "/" + uav_name + "/hw_api/arming");
+  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_midair =
+      mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh, "/" + uav_name + "/uav_manager/midair_activation");
+  mrs_lib::ServiceClientHandler<mrs_msgs::Vec4> sch_goto = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh, "/" + uav_name + "/control_manager/goto");
+
+  std::cout << "[Test]: service clients initialized" << std::endl;
+
+  // | ---------------- wait for ready to takeoff --------------- |
+
+  while (ros::ok()) {
+
+    std::cout << "[Test]: waiting for 'ready to midair activation'" << std::endl;
+
+    if (sh_control_manager_diag.hasMsg() && sh_estim_manager_diag.hasMsg()) {
+      break;
+    }
+
+    ros::spinOnce();
+    ros::Duration(0.1).sleep();
+  }
+
+  std::cout << "[Test]: we are ready 'for midair activation'" << std::endl;
+
+  ros::Duration(1.0).sleep();
+
+  // | ---------------------- arm the drone --------------------- |
+
+  std::cout << "[Test]: arming the drone" << std::endl;
+
+  std_srvs::SetBool arming;
+  arming.request.data = true;
+
+  sch_arming.call(arming);
+
+  // | -------------------------- wait -------------------------- |
+
+  ros::Duration(0.2).sleep();
+
+  // | -------------------- midair activation ------------------- |
+
+  std::cout << "[Test]: activating the drone 'in mid air'" << std::endl;
+
+  std_srvs::Trigger midair;
+
+  sch_midair.call(midair);
+
+  // | ----------------- sleep before the action ---------------- |
+
+  ros::Duration(1.0).sleep();
+
+  // --------------------------------------------------------------
+  // |                          test goto                         |
+  // --------------------------------------------------------------
+
+  mrs_msgs::Vec4 goto_cmd;
+
+  goto_cmd.request.goal[0] = -10;
+  goto_cmd.request.goal[1] = -20;
+  goto_cmd.request.goal[2] = 5.5;
+  goto_cmd.request.goal[3] = 2.2;
+
+  std::cout << "[Test]: calling goto" << std::endl;
+
+  sch_goto.call(goto_cmd);
+
+  // | ------------------ check for the result ------------------ |
+
+  while (ros::ok()) {
+
+    std::cout << "[Test]: waiting for the goto" << std::endl;
+
+    auto diag = sh_estim_manager_diag.getMsg();
+
+    auto hdg = mrs_lib::AttitudeConverter(diag->pose.orientation).getHeading();
+
+    auto flying_normally = sh_control_manager_diag.getMsg()->flying_normally;
+
+    if (std::abs(goto_cmd.request.goal[0] - diag->pose.position.x) < 1e-1 && std::abs(goto_cmd.request.goal[1] - diag->pose.position.y) < 1e-1 &&
+        std::abs(goto_cmd.request.goal[2] - diag->pose.position.z) < 1e-1 && std::abs(goto_cmd.request.goal[3] - hdg) < 1e-1 && flying_normally) {
+      result = 1;
+      break;
+    }
+
+    ros::spinOnce();
+    ros::Duration(1.0).sleep();
+  }
+
+  ROS_INFO("[%s]: finished", ros::this_node::getName().c_str());
+
+  EXPECT_TRUE(result);
+}
+
+//}
+
+int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {
+
+  ros::init(argc, argv, "goto_test");
+
+  testing::InitGoogleTest(&argc, argv);
+
+  return RUN_ALL_TESTS();
+}

test/run_test.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+
+set -e
+
+trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG
+trap 'echo "$0: \"${last_command}\" command failed with exit code $?"' ERR
+
+PACKAGE="mrs_uav_managers"
+VERBOSE=1
+
+[ "$VERBOSE" = "0" ] && TEXT_OUTPUT=""
+[ "$VERBOSE" = "1" ] && TEXT_OUTPUT="-t"
+
+# build the package
+catkin build $PACKAGE # it has to be fully built normally before building with --catkin-make-args tests
+catkin build $PACKAGE --catkin-make-args tests
+
+# folder for test results
+TEST_RESULT_PATH=$(realpath /tmp/$RANDOM)
+mkdir -p $TEST_RESULT_PATH
+
+# run the test
+rostest $PACKAGE control_manager_goto.test $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH"
+
+# evaluate the test results
+echo test result path is $TEST_RESULT_PATH
+catkin_test_results "$TEST_RESULT_PATH"
+success=$?
+
+echo ""
+if [ $success = "0" ]; then
+  echo -e "\033[0;32mSuccess! \033[1;"
+else
+  echo -e "\033[0;31mFailed some tests! \033[1;"
+fi

test/sessions/x500_in_mid_air/config/custom_config.yaml

@@ -0,0 +1,16 @@
+# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING:
+## --------------------------------------------------------------
+## |          rosrun mrs_uav_core get_public_params.py          #
+## --------------------------------------------------------------
+
+mrs_uav_managers:
+
+  estimation_manager:
+
+    # loaded state estimator plugins
+    state_estimators: [
+      "gps_baro",
+    ]
+
+    initial_state_estimator: "gps_baro" # will be used as the first state estimator
+    agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback)

test/sessions/x500_in_mid_air/config/network_config.yaml

@@ -0,0 +1,15 @@
+# 1. This list is used by NimbroNetwork for mutual communication of the UAVs
+#    The names of the robots have to match hostnames described in /etc/hosts.
+#
+# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs.
+#    The names should match the true "UAV_NAMES" (the topic prefixes).
+#
+#     network_config:=~/config/network_config.yaml
+#
+#    to the core.launch and nimbro.launch.
+
+network:
+
+  robot_names: [
+    uav1,
+  ]

test/sessions/x500_in_mid_air/config/simulator.yaml

@@ -0,0 +1,20 @@
+simulation_rate: 250.0
+clock_rate: 250.0
+realtime_factor: 1.0
+
+iterate_without_input: true
+
+individual_takeoff_platform:
+  enabled: true
+
+uav_names: [
+  "uav1",
+]
+
+uav1:
+  type: "x500"
+  spawn:
+    x: 10.0
+    y: 15.0
+    z: 2.0
+    heading: 0

test/sessions/x500_in_mid_air/config/world_config.yaml

@@ -0,0 +1,34 @@
+world_origin:
+
+  units: "LATLON" # {"UTM, "LATLON"}
+
+  origin_x: 47.397743
+  origin_y: 8.545594
+
+safety_area:
+
+  enabled: true
+
+  horizontal:
+
+    # the frame of reference in which the points are expressed
+    frame_name: "world_origin"
+
+    # polygon
+    #
+    # x, y [m] for any frame_name except latlon_origin
+    # x = latitude, y = longitude [deg]  for frame_name=="latlon_origin"
+    points: [
+      -50, -50,
+      50,  -50,
+      50,  50,
+      -50, 50,
+    ]
+
+  vertical:
+
+    # the frame of reference in which the max&min z is expressed
+    frame_name: "world_origin"
+
+    max_z: 15.0
+    min_z: 0.5