From 12e0ea4c512b170c34849ab48980ef9d60e5109c Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar@logivations.com>
Date: Tue, 18 Apr 2023 13:40:13 +0200
Subject: [PATCH] Add BT conversion TransformStamped

---
 .../nav2_behavior_tree/bt_conversions.hpp     | 29 +++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
index c109c4f0e29..2635973919d 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp
@@ -22,6 +22,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
 
 namespace BT
 {
@@ -100,6 +101,34 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
   }
 }
 
+/**
+ * @brief Parse XML string to geometry_msgs::msg::TransformStamped
+ * @param key XML string
+ * @return geometry_msgs::msg::TransformStamped
+ */
+template<>
+inline geometry_msgs::msg::TransformStamped convertFromString(const StringView key)
+{
+  // 7 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 10) {
+    throw std::runtime_error("invalid number of fields for TransformStamped attribute)");
+  } else {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    transform_stamped.header.frame_id = BT::convertFromString<std::string>(parts[1]);
+    transform_stamped.child_frame_id = BT::convertFromString<std::string>(parts[2]);
+    transform_stamped.transform.translation.x = BT::convertFromString<double>(parts[3]);
+    transform_stamped.transform.translation.y = BT::convertFromString<double>(parts[4]);
+    transform_stamped.transform.translation.z = BT::convertFromString<double>(parts[5]);
+    transform_stamped.transform.rotation.x = BT::convertFromString<double>(parts[6]);
+    transform_stamped.transform.rotation.y = BT::convertFromString<double>(parts[7]);
+    transform_stamped.transform.rotation.z = BT::convertFromString<double>(parts[8]);
+    transform_stamped.transform.rotation.w = BT::convertFromString<double>(parts[9]);
+    return transform_stamped;
+  }
+}
+
 /**
  * @brief Parse XML string to std::chrono::milliseconds
  * @param key XML string