From 9f2c58e38ece91b2eebf265d6f4faeaa3a1e5e7d Mon Sep 17 00:00:00 2001
From: Marlon Rivera <Marlonriv90@gmail.com>
Date: Fri, 6 Sep 2024 17:30:42 +0200
Subject: [PATCH 1/2] Added TransformReferenceList service into control manager

---
 launch/control_manager.launch           |  1 +
 src/control_manager/control_manager.cpp | 45 +++++++++++++++++++++++++
 2 files changed, 46 insertions(+)

diff --git a/launch/control_manager.launch b/launch/control_manager.launch
index cece6b0..a01e20e 100644
--- a/launch/control_manager.launch
+++ b/launch/control_manager.launch
@@ -101,6 +101,7 @@
       <remap from="~set_heading_in" to="~set_heading" />
       <remap from="~set_heading_relative_in" to="~set_heading_relative" />
       <remap from="~transform_reference_in" to="~transform_reference" />
+      <remap from="~transform_reference_list_in" to="~transform_reference_list" />
       <remap from="~transform_pose_in" to="~transform_pose" />
       <remap from="~transform_vector3_in" to="~transform_vector3" />
       <remap from="~validate_reference_in" to="~validate_reference" />
diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
index 3d354cb..3a75d01 100644
--- a/src/control_manager/control_manager.cpp
+++ b/src/control_manager/control_manager.cpp
@@ -93,6 +93,10 @@
 #include <mrs_msgs/TransformReferenceSrvRequest.h>
 #include <mrs_msgs/TransformReferenceSrvResponse.h>
 
+#include <mrs_msgs/TransformReferenceListSrv.h>
+#include <mrs_msgs/TransformReferenceListSrvRequest.h>
+#include <mrs_msgs/TransformReferenceListSrvResponse.h>
+
 #include <mrs_msgs/TransformPoseSrv.h>
 #include <mrs_msgs/TransformPoseSrvRequest.h>
 #include <mrs_msgs/TransformPoseSrvResponse.h>
@@ -471,6 +475,7 @@ class ControlManager : public nodelet::Nodelet {
 
   // transform service servers
   ros::ServiceServer service_server_transform_reference_;
+  ros::ServiceServer service_server_transform_reference_list_;
   ros::ServiceServer service_server_transform_pose_;
   ros::ServiceServer service_server_transform_vector3_;
 
@@ -645,6 +650,7 @@ class ControlManager : public nodelet::Nodelet {
 
   // transformation callbacks
   bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+  bool callbackTransformReferenceList(mrs_msgs::TransformReferenceListSrv::Request& req, mrs_msgs::TransformReferenceListSrv::Response& res);
   bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
   bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
 
@@ -1838,6 +1844,7 @@ void ControlManager::initialize(void) {
   service_server_parachute_                  = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
   service_server_set_min_z_                  = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
   service_server_transform_reference_        = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+  service_server_transform_reference_list_   = nh_.advertiseService("transform_reference_list_in", &ControlManager::callbackTransformReferenceList, this);
   service_server_transform_pose_             = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
   service_server_transform_vector3_          = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
   service_server_bumper_enabler_             = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
@@ -4983,6 +4990,44 @@ bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv:
   return true;
 }
 
+/* //{ callbackTransformReferenceList() */
+
+bool ControlManager::callbackTransformReferenceList(mrs_msgs::TransformReferenceListSrv::Request& req, mrs_msgs::TransformReferenceListSrv::Response& res) {
+
+  if (!is_initialized_) {
+    return false;
+  }
+
+  // transform the reference list to the current frame
+  mrs_msgs::ReferenceStamped transformed_reference; 
+  mrs_msgs::ReferenceList original_reference_list = req.list; 
+  mrs_msgs::ReferenceList transformed_reference_list;
+
+  for (size_t i=0; i < original_reference_list.list.size(); i++) {
+
+    transformed_reference.header = original_reference_list.header;
+    transformed_reference.reference = original_reference_list.list.at(i);
+
+    if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+
+      transformed_reference_list.list.push_back(ret.value().reference);
+      transformed_reference_list.header = ret.value().header;
+
+    } else {
+
+      res.message = "The reference list could not be transformed";
+      res.success = false;
+      return true;
+    }
+
+  }
+
+  res.list = transformed_reference_list;
+  res.message   = "transformation successful";
+  res.success   = true;
+  return true;
+}
+
 //}
 
 /* //{ callbackTransformPose() */

From 139faebe3ceeac7a3c2496a2db29e530169e0e99 Mon Sep 17 00:00:00 2001
From: Marlon Rivera <Marlonriv90@gmail.com>
Date: Fri, 6 Sep 2024 17:38:20 +0200
Subject: [PATCH 2/2] Fixed typo

---
 src/control_manager/control_manager.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
index 3a75d01..d32f729 100644
--- a/src/control_manager/control_manager.cpp
+++ b/src/control_manager/control_manager.cpp
@@ -4990,6 +4990,8 @@ bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv:
   return true;
 }
 
+//}
+
 /* //{ callbackTransformReferenceList() */
 
 bool ControlManager::callbackTransformReferenceList(mrs_msgs::TransformReferenceListSrv::Request& req, mrs_msgs::TransformReferenceListSrv::Response& res) {