From 91d464bf9bbcb6a2cbed3a620f2bc036b7426e7f Mon Sep 17 00:00:00 2001
From: Bastian Hofmann <bastian.hofmann@xitaso.com>
Date: Fri, 31 Mar 2023 16:33:49 +0000
Subject: [PATCH] Added another string to Pose conversion

---
 src/btree_nodes/src/Extensions.cpp | 19 +++++++++++++------
 1 file changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/btree_nodes/src/Extensions.cpp b/src/btree_nodes/src/Extensions.cpp
index 3b5fe2a..f4827ab 100644
--- a/src/btree_nodes/src/Extensions.cpp
+++ b/src/btree_nodes/src/Extensions.cpp
@@ -38,15 +38,22 @@ namespace BT {
     template<>
     std::shared_ptr<geometry_msgs::msg::Pose> convertFromString(StringView str) {
         auto parts = splitString(str, ',');
-        if (parts.size() != 3) {
-            throw RuntimeError("Incorrect number of arguments, expected 3 in format '<x>,<y>,<z>'.");
+        if (!(parts.size() == 3 || parts.size() == 7)) {
+            throw RuntimeError("Incorrect number of arguments, expected 3 or 7 in format '<x>,<y>,<z>[,<qw>,<qx>,<qy>,<qz>]'.");
         }
 
         auto pose = std::make_shared<geometry_msgs::msg::Pose>();
-        pose->orientation.w = 1;
-        pose->orientation.x = 0;
-        pose->orientation.y = 0;
-        pose->orientation.z = 0;
+        if(parts.size() == 7){
+            pose->orientation.w = convertFromString<double>(parts[3]);
+            pose->orientation.x = convertFromString<double>(parts[4]);
+            pose->orientation.y = convertFromString<double>(parts[5]);
+            pose->orientation.z = convertFromString<double>(parts[6]);
+        }else{
+            pose->orientation.w = 1;
+            pose->orientation.x = 0;
+            pose->orientation.y = 0;
+            pose->orientation.z = 0;
+        }
         pose->position.x = convertFromString<double>(parts[0]);
         pose->position.y = convertFromString<double>(parts[1]);
         pose->position.z = convertFromString<double>(parts[2]);