Merge branch 'master' of https://github.com/ctu-mrs/mrs_uav_managers

CMakeLists.txt

@@ -15,6 +15,7 @@ set(CATKIN_DEPENDENCIES
   tf2
   tf2_ros
   tf2_geometry_msgs
+  mrs_uav_testing
   )
 
 find_package(catkin REQUIRED COMPONENTS
@@ -24,7 +25,14 @@ find_package(catkin REQUIRED COMPONENTS
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
+add_definitions(-Wall)
+add_definitions(-Wextra)
+
+if(COVERAGE)
+  message(WARNING "building with --coverage, the performance might be limited")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
+endif()
 
 set(LIBRARIES
   MrsUavManagers_GainManager
@@ -163,9 +171,6 @@ target_link_libraries(MrsUavManagers_TransformManager
 
 if(CATKIN_ENABLE_TESTING)
 
-  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} --coverage")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} --coverage")
-
   add_subdirectory(test)
 
 endif()

package.xml

@@ -26,6 +26,9 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>rostest</depend>
+  <depend>mrs_uav_testing</depend>
+
+  <test_depend>mrs_uav_gazebo_simulation</test_depend>
 
   <export>
     <nodelet plugin="${prefix}/nodelets.xml" />

src/control_manager/control_manager.cpp

@@ -2869,16 +2869,10 @@ void ControlManager::timerEland(const ros::TimerEvent& event) {
     return;
   }
 
-  double throttle = 0;
-
-  if (std::holds_alternative<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value())) {
-    throttle = std::get<mrs_msgs::HwApiAttitudeCmd>(last_control_output.control_output.value()).throttle;
-  } else if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value())) {
-    throttle = std::get<mrs_msgs::HwApiAttitudeRateCmd>(last_control_output.control_output.value()).throttle;
-  } else if (std::holds_alternative<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value())) {
-    throttle = std::get<mrs_msgs::HwApiControlGroupCmd>(last_control_output.control_output.value()).throttle;
-  } else {
-    ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement eland timer for this control mode");
+  auto throttle = extractThrottle(last_control_output);
+
+  if (!throttle) {
+    ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
     return;
   }
 
@@ -2888,14 +2882,22 @@ void ControlManager::timerEland(const ros::TimerEvent& event) {
 
   } else if (current_state_landing_ == LANDING_STATE) {
 
+    // --------------------------------------------------------------
+    // |                            TODO                            |
+    // This section needs work. The throttle landing detection      |
+    // mechanism should be extracted and other mechanisms, such     |
+    // as velocity-based detection should be used for high          |
+    // modalities                                                   |
+    // --------------------------------------------------------------
+
     if (!last_control_output.control_output) {
       ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
       ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
       return;
     }
 
     // recalculate the mass based on the throttle
-    throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle) / common_handlers_->g;
+    throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
     ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
 
     // condition for automatic motor turn off
@@ -2977,6 +2979,14 @@ void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
     return;
   }
 
+  // --------------------------------------------------------------
+  // |                            TODO                            |
+  // This section needs work. The throttle landing detection      |
+  // mechanism should be extracted and other mechanisms, such     |
+  // as velocity-based detection should be used for high          |
+  // modalities                                                   |
+  // --------------------------------------------------------------
+
   double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
   ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
 
@@ -7259,6 +7269,7 @@ std::tuple<bool, std::string> ControlManager::eland(void) {
 /* failsafe() //{ */
 
 std::tuple<bool, std::string> ControlManager::failsafe(void) {
+
   // copy member variables
   auto last_control_output   = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
   auto last_tracker_cmd      = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
@@ -7288,6 +7299,10 @@ std::tuple<bool, std::string> ControlManager::failsafe(void) {
     return std::tuple(true, "RC emergency handoff is ON, disabling output");
   }
 
+  if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+    return eland();
+  }
+
   if (_parachute_enabled_) {
 
     auto [success, message] = deployParachute();

test/CMakeLists.txt

@@ -1,127 +1,5 @@
 find_package(rostest REQUIRED)
 
-# takeoff test
+add_subdirectory(control_manager)
 
-add_rostest_gtest(MrsUavManagers_UavManagerTakeoffTest
-  uav_manager/takeoff/uav_manager_takeoff.test
-  uav_manager/takeoff/takeoff_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_UavManagerTakeoffTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_UavManagerTakeoffTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# landing test
-
-add_rostest_gtest(MrsUavManagers_UavManagerLandingTest
-  uav_manager/landing/uav_manager_landing.test
-  uav_manager/landing/landing_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_UavManagerLandingTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_UavManagerLandingTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# goto service test
-
-add_rostest_gtest(MrsUavManagers_ControlManagerGotoServiceTest
-  control_manager/goto_service/control_manager_goto_service.test
-  control_manager/goto_service/goto_service_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_ControlManagerGotoServiceTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_ControlManagerGotoServiceTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# eland innovation test
-
-add_rostest_gtest(MrsUavManagers_ControlManagerElandInnovationTest
-  control_manager/eland_innovation/control_manager_eland_innovation.test
-  control_manager/eland_innovation/eland_innovation_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_ControlManagerElandInnovationTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_ControlManagerElandInnovationTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# eland controlerror test
-
-add_rostest_gtest(MrsUavManagers_ControlManagerElandControlErrorTest
-  control_manager/eland_control_error/control_manager_eland_control_error.test
-  control_manager/eland_control_error/eland_control_error_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_ControlManagerElandControlErrorTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_ControlManagerElandControlErrorTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# failsafe controlerror test
-
-add_rostest_gtest(MrsUavManagers_ControlManagerFailsafeControlErrorTest
-  control_manager/failsafe_control_error/control_manager_failsafe_control_error.test
-  control_manager/failsafe_control_error/failsafe_control_error_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_ControlManagerFailsafeControlErrorTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_ControlManagerFailsafeControlErrorTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
-
-# eland service test
-
-add_rostest_gtest(MrsUavManagers_ControlManagerElandServiceTest
-  control_manager/eland_service/control_manager_eland_service.test
-  control_manager/eland_service/eland_service_test.cpp
-  )
-
-target_link_libraries(
-  MrsUavManagers_ControlManagerElandServiceTest
-  ${catkin_LIBRARIES}
-  )
-
-add_dependencies(
-  MrsUavManagers_ControlManagerElandServiceTest
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  )
+add_subdirectory(uav_manager)

test/all_tests.sh

@@ -5,31 +5,30 @@ 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 --no-deps --catkin-make-args tests
-
-TEST_FILES=$(find . -name "*.test" -type f -printf "%f\n")
+ORIGINAL_PATH=`pwd`
+
+while [ ! -e ".catkin_tools" ]; do
+  cd ..
+  if [[ `pwd` == "/" ]]; then
+    # we reached the root and didn't find the build/COLCON_IGNORE file - that's a fail!
+    echo "$0: could not find the root of the current workspace".
+    return 1
+  fi
+done
 
-for TEST_FILE in `echo $TEST_FILES`; do
+cd build
 
-  echo "$0: running test '$TEST_FILE'"
+OLD_FILES=$(find . -name "*.gcda")
 
-  # folder for test results
-  TEST_RESULT_PATH=$(realpath /tmp/$RANDOM)
-  mkdir -p $TEST_RESULT_PATH
+for FILE in $OLD_FILES; do
+  echo "$0: removing old coverage file '$FILE'"
+  rm $FILE
+done
 
-  # run the test
-  rostest $PACKAGE $TEST_FILE $TEXT_OUTPUT --results-filename=$PACKAGE.test --results-base-dir="$TEST_RESULT_PATH"
+cd $ORIGINAL_PATH
 
-  # evaluate the test results
-  echo test result path is $TEST_RESULT_PATH
-  catkin_test_results "$TEST_RESULT_PATH"
+# build the package
+catkin build --this # it has to be fully built normally before building with --catkin-make-args tests
+catkin build --this --no-deps --catkin-make-args tests
 
-done
+catkin test --this -i -p 1 -s

test/control_manager/CMakeLists.txt

@@ -0,0 +1,7 @@
+add_subdirectory(goto_service)
+
+add_subdirectory(eland_control_error)
+
+add_subdirectory(eland_innovation)
+
+add_subdirectory(failsafe_control_error)

test/control_manager/eland_control_error/CMakeLists.txt

@@ -0,0 +1,16 @@
+get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME)
+
+catkin_add_executable_with_gtest(test_${TEST_NAME}
+  test.cpp
+  )
+
+target_link_libraries(test_${TEST_NAME}
+  ${catkin_LIBRARIES}
+  )
+
+add_dependencies(test_${TEST_NAME}
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  )
+
+add_rostest(${TEST_NAME}.test)

test/control_manager/eland_control_error/config/custom_config.yaml

@@ -9,10 +9,10 @@ mrs_uav_managers:
 
     # loaded state estimator plugins
     state_estimators: [
-      "passthrough",
+      "gps_baro",
     ]
 
-    initial_state_estimator: "passthrough" # will be used as the first state estimator
+    initial_state_estimator: "gps_baro" # will be used as the first state estimator
     agl_height_estimator: "" # only slightly filtered height for checking min height (not used in control feedback)
 
   control_manager: