added reference validation test

ctu-mrs · Apr 29, 2024 · 8b10b96 · 8b10b96
1 parent 6ac5319
commit 8b10b96
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 0 deletions.
diff --git a/include/mrs_uav_testing/test_generic.h b/include/mrs_uav_testing/test_generic.h
@@ -35,6 +35,8 @@
 #include <mrs_msgs/VelocityReferenceStamped.h>
 #include <mrs_msgs/VelocityReferenceSrv.h>
 #include <mrs_msgs/ReferenceStampedSrv.h>
+#include <mrs_msgs/ValidateReference.h>
+#include <mrs_msgs/ValidateReferenceList.h>
 
 #include <std_srvs/SetBool.h>
 #include <std_srvs/Trigger.h>
@@ -124,6 +126,8 @@ class UAVHandler {
 
   bool mrsSystemReady(void);
 
+  tuple<bool, string> validateReference(const mrs_msgs::ReferenceStamped &msg);
+
   mrs_lib::SubscribeHandler<mrs_msgs::ControlManagerDiagnostics>    sh_control_manager_diag_;
   mrs_lib::SubscribeHandler<mrs_msgs::DynamicsConstraints>          sh_current_constraints_;
   mrs_lib::SubscribeHandler<mrs_msgs::UavManagerDiagnostics>        sh_uav_manager_diag_;
@@ -162,6 +166,9 @@ class UAVHandler {
   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_resume_trajectory_tracking_;
   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_goto_trajectory_start_;
 
+  mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReference>     sch_validate_reference_;
+  mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReferenceList> sch_validate_reference_list_;
+
   mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_hover_;
 
   mrs_lib::PublisherHandler<mrs_msgs::Path>                     ph_path_;

diff --git a/src/test_generic.cpp b/src/test_generic.cpp
@@ -65,6 +65,10 @@ void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptio
   sch_path_     = mrs_lib::ServiceClientHandler<mrs_msgs::PathSrv>(nh_, "/" + _uav_name_ + "/trajectory_generation/path");
   sch_get_path_ = mrs_lib::ServiceClientHandler<mrs_msgs::GetPathSrv>(nh_, "/" + _uav_name_ + "/trajectory_generation/get_path");
 
+  sch_validate_reference_ = mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReference>(nh_, "/" + _uav_name_ + "/control_manager/validate_reference");
+  sch_validate_reference_list_ =
+      mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReferenceList>(nh_, "/" + _uav_name_ + "/control_manager/validate_reference_list");
+
   // | ----------------------- publishers ----------------------- |
 
   ph_path_               = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "/" + _uav_name_ + "/trajectory_generation/path");
@@ -1370,6 +1374,32 @@ tuple<std::optional<mrs_msgs::TrajectoryReference>, string> UAVHandler::getPathS
 
 //}
 
+/* validateReference() //{ */
+
+tuple<bool, string> UAVHandler::validateReference(const mrs_msgs::ReferenceStamped &msg) {
+
+  auto res = checkPreconditions();
+
+  if (!(std::get<0>(res))) {
+    return res;
+  }
+
+  mrs_msgs::ValidateReference srv;
+  srv.request.reference = msg;
+
+  {
+    bool service_call = sch_validate_reference_.call(srv);
+
+    if (!service_call) {
+      return {false, "reference validation service call failed"};
+    } else {
+      return {srv.response.success, srv.response.message};
+    }
+  }
+}
+
+//}
+
 /* sleep() //{ */
 
 void UAVHandler::sleep(const double &duration) {