added endpoints for transformation services

ctu-mrs · May 6, 2024 · 04ff9c7 · 04ff9c7
1 parent 2f29401
commit 04ff9c7
Show file tree

Hide file tree

Showing 2 changed files with 104 additions and 1 deletion.
diff --git a/include/mrs_uav_testing/test_generic.h b/include/mrs_uav_testing/test_generic.h
@@ -37,6 +37,9 @@
 #include <mrs_msgs/ReferenceStampedSrv.h>
 #include <mrs_msgs/ValidateReference.h>
 #include <mrs_msgs/ValidateReferenceList.h>
+#include <mrs_msgs/TransformReferenceSrv.h>
+#include <mrs_msgs/TransformVector3Srv.h>
+#include <mrs_msgs/TransformPoseSrv.h>
 
 #include <std_srvs/SetBool.h>
 #include <std_srvs/Trigger.h>
@@ -124,9 +127,17 @@ class UAVHandler {
 
   bool mrsSystemReady(void);
 
-  tuple<bool, string>                                                   validateReference(const mrs_msgs::ReferenceStamped &msg);
+  tuple<bool, string> validateReference(const mrs_msgs::ReferenceStamped &msg);
+
   tuple<bool, std::optional<mrs_msgs::ValidateReferenceList::Response>> validateReferenceList(const mrs_msgs::ValidateReferenceList::Request &request);
 
+  std::tuple<bool, std::optional<std::string>, std::optional<geometry_msgs::PoseStamped>> transformPose(const geometry_msgs::PoseStamped &msg,
+                                                                                                        std::string                       target_frame);
+  tuple<bool, std::optional<std::string>, std::optional<mrs_msgs::ReferenceStamped>>      transformReference(const mrs_msgs::ReferenceStamped &msg,
+                                                                                                             std::string                       target_frame);
+  tuple<bool, std::optional<std::string>, std::optional<geometry_msgs::Vector3Stamped>>   transformVector3(const geometry_msgs::Vector3Stamped &msg,
+                                                                                                           std::string                          target_frame);
+
   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_;
@@ -168,6 +179,10 @@ class UAVHandler {
   mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReference>     sch_validate_reference_;
   mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReferenceList> sch_validate_reference_list_;
 
+  mrs_lib::ServiceClientHandler<mrs_msgs::TransformReferenceSrv> sch_tranform_reference_;
+  mrs_lib::ServiceClientHandler<mrs_msgs::TransformVector3Srv>   sch_tranform_vector3_;
+  mrs_lib::ServiceClientHandler<mrs_msgs::TransformPoseSrv>      sch_tranform_pose_;
+
   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
@@ -68,6 +68,10 @@ void UAVHandler::initialize(std::string uav_name, mrs_lib::SubscribeHandlerOptio
   sch_validate_reference_list_ =
       mrs_lib::ServiceClientHandler<mrs_msgs::ValidateReferenceList>(nh_, "/" + _uav_name_ + "/control_manager/validate_reference_list");
 
+  sch_tranform_reference_ = mrs_lib::ServiceClientHandler<mrs_msgs::TransformReferenceSrv>(nh_, "/" + _uav_name_ + "/control_manager/transform_reference");
+  sch_tranform_vector3_   = mrs_lib::ServiceClientHandler<mrs_msgs::TransformVector3Srv>(nh_, "/" + _uav_name_ + "/control_manager/transform_vector3");
+  sch_tranform_pose_      = mrs_lib::ServiceClientHandler<mrs_msgs::TransformPoseSrv>(nh_, "/" + _uav_name_ + "/control_manager/transform_pose");
+
   // | ----------------------- publishers ----------------------- |
 
   ph_path_               = mrs_lib::PublisherHandler<mrs_msgs::Path>(nh_, "/" + _uav_name_ + "/trajectory_generation/path");
@@ -1400,6 +1404,90 @@ tuple<bool, string> UAVHandler::validateReference(const mrs_msgs::ReferenceStamp
 
 //}
 
+/* transformReference() //{ */
+
+std::tuple<bool, std::optional<std::string>, std::optional<mrs_msgs::ReferenceStamped>> UAVHandler::transformReference(const mrs_msgs::ReferenceStamped &msg,
+                                                                                                                       std::string target_frame) {
+
+  auto res = checkPreconditions();
+
+  if (!(std::get<0>(res))) {
+    return {};
+  }
+
+  mrs_msgs::TransformReferenceSrv srv;
+  srv.request.reference = msg;
+  srv.request.frame_id  = target_frame;
+
+  {
+    bool service_call = sch_tranform_reference_.call(srv);
+
+    if (!service_call) {
+      return {false, "Reference validation service call failed", {}};
+    } else {
+      return {srv.response.success, srv.response.message, srv.response.reference};
+    }
+  }
+}
+
+//}
+
+/* transformPose() //{ */
+
+std::tuple<bool, std::optional<std::string>, std::optional<geometry_msgs::PoseStamped>> UAVHandler::transformPose(const geometry_msgs::PoseStamped &msg,
+                                                                                                                  std::string target_frame) {
+
+  auto res = checkPreconditions();
+
+  if (!(std::get<0>(res))) {
+    return {};
+  }
+
+  mrs_msgs::TransformPoseSrv srv;
+  srv.request.pose     = msg;
+  srv.request.frame_id = target_frame;
+
+  {
+    bool service_call = sch_tranform_pose_.call(srv);
+
+    if (!service_call) {
+      return {false, "Pose validation service call failed", {}};
+    } else {
+      return {srv.response.success, srv.response.message, srv.response.pose};
+    }
+  }
+}
+
+//}
+
+/* transformVector3() //{ */
+
+std::tuple<bool, std::optional<std::string>, std::optional<geometry_msgs::Vector3Stamped>> UAVHandler::transformVector3(
+    const geometry_msgs::Vector3Stamped &msg, std::string target_frame) {
+
+  auto res = checkPreconditions();
+
+  if (!(std::get<0>(res))) {
+    return {};
+  }
+
+  mrs_msgs::TransformVector3Srv srv;
+  srv.request.vector   = msg;
+  srv.request.frame_id = target_frame;
+
+  {
+    bool service_call = sch_tranform_vector3_.call(srv);
+
+    if (!service_call) {
+      return {false, "Vector3 validation service call failed", {}};
+    } else {
+      return {srv.response.success, srv.response.message, srv.response.vector};
+    }
+  }
+}
+
+//}
+
 /* validateReferenceList() //{ */
 
 tuple<bool, std::optional<mrs_msgs::ValidateReferenceList::Response>> UAVHandler::validateReferenceList(