fix bt_utils

Signed-off-by: Tony Najjar <[email protected]>

Author: tonynajjar

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

@@ -104,6 +104,36 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
   }
 }
 
+/**
+ * @brief Parse XML string to std::vector<geometry_msgs::msg::PoseStamped>
+ * @param key XML string
+ * @return std::vector<geometry_msgs::msg::PoseStamped>
+ */
+template<>
+inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
+{
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() % 9 != 0) {
+    throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
+  } else {
+    std::vector<geometry_msgs::msg::PoseStamped> poses;
+    for (size_t i = 0; i < parts.size(); i += 9) {
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
+      pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
+      pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
+      pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
+      pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
+      pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
+      pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
+      pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
+      pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
+      poses.push_back(pose_stamped);
+    }
+    return poses;
+  }
+}
+
 /**
  * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray
  * @param key XML string
@@ -112,13 +142,14 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
 template<>
 inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key)
 {
-  // 9 real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
-  if (parts.size() % 9 != 0) {
+  if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)");
   } else {
     geometry_msgs::msg::PoseStampedArray pose_stamped_array;
-    for (size_t i = 0; i < parts.size(); i += 9) {
+    pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    pose_stamped_array.header.frame_id = BT::convertFromString<std::string>(parts[1]);
+    for (size_t i = 2; i < parts.size(); i += 9) {
       geometry_msgs::msg::PoseStamped pose_stamped;
       pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
       pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
@@ -143,7 +174,6 @@ inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView k
 template<>
 inline nav_msgs::msg::Path convertFromString(const StringView key)
 {
-  // 9 real numbers separated by semicolons
   auto parts = BT::splitString(key, ';');
   if ((parts.size() - 2) % 9 != 0) {
     throw std::runtime_error("invalid number of fields for Path attribute)");

nav2_behavior_tree/test/test_bt_utils.cpp

@@ -194,13 +194,77 @@ TEST(PoseStampedPortTest, test_correct_syntax)
   EXPECT_EQ(value.pose.orientation.w, 7.0);
 }
 
+TEST(PoseStampedVectorPortTest, test_wrong_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PoseStampedVectorPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<std::vector<geometry_msgs::msg::PoseStamped>>>(
+    "PoseStampedVectorPortTest");
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+
+  xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PoseStampedVectorPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
+        </BehaviorTree>
+      </root>)";
+
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+}
+
+TEST(PoseStampedVectorPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PoseStampedVectorPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;odom;8.0;9.0;10.0;11.0;12.0;13.0;14.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<std::vector<geometry_msgs::msg::PoseStamped>>>(
+    "PoseStampedVectorPortTest");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  tree = factory.createTreeFromText(xml_txt);
+  std::vector<geometry_msgs::msg::PoseStamped> values;
+  tree.rootNode()->getInput("test", values);
+  EXPECT_EQ(rclcpp::Time(values[0].header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(values[0].header.frame_id, "map");
+  EXPECT_EQ(values[0].pose.position.x, 1.0);
+  EXPECT_EQ(values[0].pose.position.y, 2.0);
+  EXPECT_EQ(values[0].pose.position.z, 3.0);
+  EXPECT_EQ(values[0].pose.orientation.x, 4.0);
+  EXPECT_EQ(values[0].pose.orientation.y, 5.0);
+  EXPECT_EQ(values[0].pose.orientation.z, 6.0);
+  EXPECT_EQ(values[0].pose.orientation.w, 7.0);
+  EXPECT_EQ(rclcpp::Time(values[1].header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(values[1].header.frame_id, "odom");
+  EXPECT_EQ(values[1].pose.position.x, 8.0);
+  EXPECT_EQ(values[1].pose.position.y, 9.0);
+  EXPECT_EQ(values[1].pose.position.z, 10.0);
+  EXPECT_EQ(values[1].pose.orientation.x, 11.0);
+  EXPECT_EQ(values[1].pose.orientation.y, 12.0);
+  EXPECT_EQ(values[1].pose.orientation.z, 13.0);
+  EXPECT_EQ(values[1].pose.orientation.w, 14.0);
+}
+
 TEST(PoseStampedArrayPortTest, test_wrong_syntax)
 {
   std::string xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <PoseStampedArrayPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
+            <PoseStampedArrayPortTest test="0;map;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
         </BehaviorTree>
       </root>)";
 
@@ -213,7 +277,7 @@ TEST(PoseStampedArrayPortTest, test_wrong_syntax)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <PoseStampedArrayPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
+            <PoseStampedArrayPortTest test="0;map;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
         </BehaviorTree>
       </root>)";
 
@@ -226,7 +290,7 @@ TEST(PoseStampedArrayPortTest, test_correct_syntax)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <PoseStampedArrayPortTest test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;odom;8.0;9.0;10.0;11.0;12.0;13.0;14.0" />
+            <PoseStampedArrayPortTest test="0;map;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;odom;8.0;9.0;10.0;11.0;12.0;13.0;14.0" />
         </BehaviorTree>
       </root>)";