From a17e28d10987a1d05b500b886708ddfdb526d3f9 Mon Sep 17 00:00:00 2001
From: rafal-gorecki <rafal.gorecki@husarion.com>
Date: Tue, 5 Nov 2024 15:10:23 +0100
Subject: [PATCH] Fix tests

---
 .../panther_manager/behavior_tree_utils.hpp   |  4 +-
 .../test/test_behavior_tree_utils.cpp         | 60 ++++++++++++++-----
 2 files changed, 46 insertions(+), 18 deletions(-)

diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp
index 4c8d25ab..782097d5 100644
--- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp
+++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp
@@ -118,9 +118,9 @@ inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
 /**
  * @brief Converts a string to a PoseStamped message.
  *
- * The string format should be "roll;pitch;yaw;x;y;z;frame_id" where:
- *  - roll, pitch, yaw: Euler angles in radians.
+ * The string format should be "x;y;z;roll;pitch;yaw;frame_id" where:
  *  - x, y, z: Position coordinates.
+ *  - roll, pitch, yaw: Euler angles in radians.
  *  - frame_id: Coordinate frame ID (string).
  *
  * @param str The string to convert.
diff --git a/panther_manager/test/test_behavior_tree_utils.cpp b/panther_manager/test/test_behavior_tree_utils.cpp
index a65c0a07..660bcf46 100644
--- a/panther_manager/test/test_behavior_tree_utils.cpp
+++ b/panther_manager/test/test_behavior_tree_utils.cpp
@@ -26,6 +26,8 @@
 #include "behaviortree_ros2/ros_node_params.hpp"
 #include "rclcpp/rclcpp.hpp"
 
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
 #include "panther_manager/behavior_tree_utils.hpp"
 #include "panther_utils/test/test_utils.hpp"
 
@@ -121,33 +123,59 @@ TEST_F(TestRegisterBT, RegisterBehaviorTreeROS)
   rclcpp::shutdown();
 }
 
-TEST(TestConvertFromString, GoodVectorInt)
+TEST(TestConvertFromStringPoseStamped, GoodInput)
 {
-  std::string str = "1;2;3";
-  auto result = BT::convertFromString<std::vector<int>>(str);
+  constexpr double time_threshold = 0.1;
+  constexpr float epsilon = 1e-3;
+
+  auto str = "1;2;3;0.1;0.2;0.1;pose";
+  auto result = BT::convertFromString<geometry_msgs::msg::PoseStamped>(str);
+
+  auto time_diff = rclcpp::Clock().now() - rclcpp::Time(result.header.stamp, RCL_SYSTEM_TIME);
+  EXPECT_LE(time_diff.seconds(), time_threshold);
+  EXPECT_EQ(result.header.frame_id, "pose");
 
-  EXPECT_EQ(result.size(), 3);
-  EXPECT_EQ(result[0], 1);
-  EXPECT_EQ(result[1], 2);
-  EXPECT_EQ(result[2], 3);
+  EXPECT_NEAR(result.pose.position.x, 1, epsilon);
+  EXPECT_NEAR(result.pose.position.y, 2, epsilon);
+  EXPECT_NEAR(result.pose.position.z, 3, epsilon);
+
+  EXPECT_NEAR(result.pose.orientation.x, 0.0447, epsilon);
+  EXPECT_NEAR(result.pose.orientation.y, 0.1021, epsilon);
+  EXPECT_NEAR(result.pose.orientation.z, 0.0447, epsilon);
+  EXPECT_NEAR(result.pose.orientation.w, 0.9928, epsilon);
 }
 
-TEST(TestConvertFromString, EmptyVectorInt)
+TEST(TestConvertFromStringPoseStamped, WrongInput)
 {
-  std::string str = "";
-  EXPECT_THROW(BT::convertFromString<std::vector<int>>(str), BT::RuntimeError);
+  auto str = "";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "1;2;3;0.1;0.2;0.1;";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "1;2;3;0.1;0.2;0.1;pose;0.1";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "pose;1;2;3;0.1;0.2;0.1;";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
 }
 
-TEST(TestConvertFromString, InvalidVectorIntWithFloat)
+TEST(TestConvertFromStringVectorOfDouble, GoodInput)
 {
-  std::string str = "1.0;2;3;4";
-  EXPECT_THROW(BT::convertFromString<std::vector<int>>(str), BT::RuntimeError);
+  constexpr float epsilon = 1e-6;
+
+  auto str = "1;2;3;0.1;0.2;0.1";
+  auto result = BT::convertFromString<std::vector<float>>(str);
+
+  EXPECT_NEAR(result[0], 1, epsilon);
+  EXPECT_NEAR(result[1], 2, epsilon);
+  EXPECT_NEAR(result[2], 3, epsilon);
+  EXPECT_NEAR(result[3], 0.1, epsilon);
+  EXPECT_NEAR(result[4], 0.2, epsilon);
+  EXPECT_NEAR(result[5], 0.1, epsilon);
 }
 
-TEST(TestConvertFromString, InvalidVectorIntWithLetter)
+TEST(TestConvertFromStringVectorOfFloat, WrongInput)
 {
-  std::string str = "a;2;3;4";
-  EXPECT_THROW(BT::convertFromString<std::vector<int>>(str), BT::RuntimeError);
+  auto str = "1;2;3;0.1;a;0.2";
+  EXPECT_ANY_THROW(BT::convertFromString<std::vector<float>>(str));
 }
 
 int main(int argc, char ** argv)