updated testing interface

ctu-mrs · Dec 17, 2023 · e2d226d · e2d226d
1 parent e2a5786
commit e2d226d
Show file tree

Hide file tree

Showing 2 changed files with 115 additions and 12 deletions.
diff --git a/include/mrs_uav_testing/test_generic.h b/include/mrs_uav_testing/test_generic.h
@@ -1,6 +1,8 @@
 #ifndef TEST_GENERIC_H
 #define TEST_GENERIC_H
 
+/* includes //{ */
+
 #include <ros/ros.h>
 #include <ros/console.h>
 #include <log4cxx/logger.h>
@@ -24,10 +26,14 @@
 #include <mrs_msgs/String.h>
 #include <mrs_msgs/UavState.h>
 #include <mrs_msgs/PathSrv.h>
+#include <mrs_msgs/Float64Stamped.h>
+#include <mrs_msgs/TrackerCommand.h>
 
 #include <std_srvs/SetBool.h>
 #include <std_srvs/Trigger.h>
 
+//}
+
 namespace mrs_uav_testing
 {
 
@@ -36,8 +42,6 @@ using sradians = mrs_lib::geometry::sradians;
 
 using namespace std;
 
-/* TestGeneric //{ */
-
 class TestGeneric {
 
 public:
@@ -57,6 +61,7 @@ class TestGeneric {
 
   tuple<bool, string> takeoff(void);
   tuple<bool, string> land(void);
+  tuple<bool, string> landHome(void);
   tuple<bool, string> activateMidAir(void);
 
   tuple<bool, string> gotoAbs(const double &x, const double &y, const double &z, const double &hdg);
@@ -65,14 +70,16 @@ class TestGeneric {
 
   void sleep(const double &duration);
 
-  bool        hasGoal(void);
-  bool        isFlyingNormally(void);
-  bool        isOutputEnabled(void);
-  bool        isAtPosition(const double &x, const double &y, const double &z, const double &hdg, const double &pos_tolerance);
+  bool hasGoal(void);
+  bool isFlyingNormally(void);
+  bool isOutputEnabled(void);
+  bool isAtPosition(const double &x, const double &y, const double &z, const double &hdg, const double &pos_tolerance);
 
-  std::string getActiveTracker(void);
-  std::string getActiveController(void);
-  std::string getActiveEstimator(void);
+  std::string                             getActiveTracker(void);
+  std::string                             getActiveController(void);
+  std::string                             getActiveEstimator(void);
+  std::optional<mrs_msgs::TrackerCommand> getTrackerCmd(void);
+  std::optional<double>                   getHeightAgl(void);
 
   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>    sh_control_manager_diag_;
   mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>        sh_uav_manager_diag_;
@@ -81,6 +88,8 @@ class TestGeneric {
   mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics> sh_constraint_manager_diag_;
   mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>     sh_gazebo_spawner_diag_;
   mrs_lib::SubscribeHandler<mrs_msgs::UavState>                     sh_uav_state_;
+  mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>               sh_tracker_cmd_;
+  mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>               sh_height_agl_;
 
   mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
 
@@ -89,6 +98,7 @@ class TestGeneric {
   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_midair_activation_;
   mrs_lib::ServiceClientHandler<mrs_msgs::String>  sch_spawn_gazebo_uav_;
   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_land_;
+  mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_land_home_;
 
   mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>    sch_goto_;
   mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv> sch_path_;
@@ -110,8 +120,6 @@ class TestGeneric {
   shared_ptr<ros::AsyncSpinner> spinner_;
 };
 
-//}
-
 }  // namespace mrs_uav_testing
 
 #endif  // TEST_GENERIC_H
diff --git a/src/test_generic.cpp b/src/test_generic.cpp
@@ -53,11 +53,13 @@ void TestGeneric::initialize(void) {
 
   sh_control_manager_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/control_manager/diagnostics");
   sh_uav_manager_diag_     = mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/uav_manager/diagnostics");
+  sh_tracker_cmd_          = mrs_lib::SubscribeHandler<mrs_msgs::TrackerCommand>(shopts_, "/" + _uav_name_ + "/control_manager/tracker_cmd");
   sh_estim_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::EstimationDiagnostics>(shopts_, "/" + _uav_name_ + "/estimation_manager/diagnostics");
   sh_constraint_manager_diag_ =
       mrs_lib::SubscribeHandler<mrs_msgs::ConstraintManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/constraint_manager/diagnostics");
   sh_gain_manager_diag_   = mrs_lib::SubscribeHandler<mrs_msgs::GainManagerDiagnostics>(shopts_, "/" + _uav_name_ + "/gain_manager/diagnostics");
   sh_uav_state_           = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts_, "/" + _uav_name_ + "/estimation_manager/uav_state");
+  sh_height_agl_          = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts_, "/" + _uav_name_ + "/estimation_manager/height_agl");
   sh_gazebo_spawner_diag_ = mrs_lib::SubscribeHandler<mrs_msgs::GazeboSpawnerDiagnostics>(shopts_, "/mrs_drone_spawner/diagnostics");
 
   sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts_, "/" + _uav_name_ + "/hw_api/status");
@@ -69,6 +71,7 @@ void TestGeneric::initialize(void) {
   sch_midair_activation_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/uav_manager/midair_activation");
   sch_spawn_gazebo_uav_  = mrs_lib::ServiceClientHandler<mrs_msgs::String>(nh_, "/mrs_drone_spawner/spawn");
   sch_land_              = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/uav_manager/land");
+  sch_land_home_         = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "/" + _uav_name_ + "/uav_manager/land_home");
 
   sch_goto_          = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh_, "/" + _uav_name_ + "/control_manager/goto");
   sch_goto_relative_ = mrs_lib::ServiceClientHandler<mrs_msgs::Vec4>(nh_, "/" + _uav_name_ + "/control_manager/goto_relative");
@@ -286,7 +289,7 @@ tuple<bool, string> TestGeneric::land(void) {
     return {false, "not flying normally in the beginning"};
   }
 
-  // | ---------------------- arm the drone --------------------- |
+  // | -------------------- call land service ------------------- |
 
   ROS_INFO("[%s]: calling for landing", name_.c_str());
 
@@ -344,6 +347,72 @@ tuple<bool, string> TestGeneric::land(void) {
 
 //}
 
+/* landHome() //{ */
+
+tuple<bool, string> TestGeneric::landHome(void) {
+
+  if (!isFlyingNormally()) {
+    return {false, "not flying normally in the beginning"};
+  }
+
+  // | ----------------- call land home service ----------------- |
+
+  ROS_INFO("[%s]: calling for landing home", name_.c_str());
+
+  {
+    std_srvs::Trigger srv;
+
+    {
+      bool service_call = sch_land_home_.call(srv);
+
+      if (!service_call || !srv.response.success) {
+        return {false, "land home service call failed"};
+      }
+    }
+  }
+
+  // | ---------------------- wait a second --------------------- |
+
+  sleep(1.0);
+
+  // | -------- wait till the right controller is active -------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return {false, "shut down from outside"};
+    }
+
+    if (sh_control_manager_diag_.getMsg()->active_tracker == "LandoffTracker" && sh_control_manager_diag_.getMsg()->active_controller == "MpcController") {
+      break;
+    }
+
+    sleep(0.01);
+  }
+
+  // | ------------- wait for the landing to finish ------------- |
+
+  while (true) {
+
+    if (!ros::ok()) {
+      return {false, "shut down from outside"};
+    }
+
+    ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the landing to finish", name_.c_str());
+
+    if (!isOutputEnabled()) {
+
+      return {true, "landing finished"};
+    }
+
+    sleep(0.01);
+  }
+
+  return {false, "reached end of the method without assertion"};
+}
+
+//}
+
 /* activateMidAir() //{ */
 
 tuple<bool, string> TestGeneric::activateMidAir(void) {
@@ -553,6 +622,32 @@ tuple<bool, string> TestGeneric::setPathTopic(const mrs_msgs::Path &path_in) {
 
 //}
 
+/* getHeightAgl() //{ */
+
+std::optional<double> TestGeneric::getHeightAgl(void) {
+
+  if (sh_height_agl_.hasMsg()) {
+    return {sh_height_agl_.getMsg()->value};
+  } else {
+    return {};
+  }
+}
+
+//}
+
+/* getTrackerCmd() //{ */
+
+std::optional<mrs_msgs::TrackerCommand> TestGeneric::getTrackerCmd(void) {
+
+  if (sh_tracker_cmd_.hasMsg()) {
+    return {*sh_tracker_cmd_.getMsg()};
+  } else {
+    return {};
+  }
+}
+
+//}
+
 // | ------------------------- getters ------------------------ |
 
 /* isAtPosition() //{ */