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 @@
+
diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp
index 3d354cb..d32f729 100644
--- a/src/control_manager/control_manager.cpp
+++ b/src/control_manager/control_manager.cpp
@@ -93,6 +93,10 @@
#include
#include
+#include
+#include
+#include
+
#include
#include
#include
@@ -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);
@@ -4985,6 +4992,46 @@ bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv:
//}
+/* //{ 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() */
bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {