add path

Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Aug 23, 2024 · 9478d5b · 9478d5b
1 parent dd57910
commit 9478d5b
Show file tree

Hide file tree

Showing 2 changed files with 101 additions and 1 deletion.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
@@ -25,6 +25,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
 
 namespace BT
 {
@@ -134,6 +135,39 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
   }
 }
 
+/**
+ * @brief Parse XML string to nav_msgs::msg::Path
+ * @param key XML string
+ * @return nav_msgs::msg::Path
+ */
+template<>
+inline nav_msgs::msg::Path convertFromString(const StringView key)
+{
+  // 9 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if ((parts.size() - 9) % 7 == 0) {
+    throw std::runtime_error("invalid number of fields for Path attribute)");
+  } else {
+    nav_msgs::msg::Path path;
+    path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    path.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;
+      path.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]);
+      path.poses.push_back(pose_stamped);
+    }
+    return path;
+  }
+}
+
 /**
  * @brief Parse XML string to std::chrono::milliseconds
  * @param key XML string

diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp
@@ -247,7 +247,7 @@ TEST(PoseStampedVectorPortTest, test_correct_syntax)
   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[0].header.stamp).nanoseconds(), 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);
@@ -258,6 +258,72 @@ TEST(PoseStampedVectorPortTest, test_correct_syntax)
   EXPECT_EQ(values[1].pose.orientation.w, 14.0);
 }
 
+TEST(PathPortTest, test_wrong_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PathPortTest 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<nav_msgs::msg::Path>>(
+    "PathPortTest");
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
+
+  xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PathPortTest 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(PathPortTest, test_correct_syntax)
+{
+  std::string xml_txt =
+    R"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+            <PathPortTest test="0;map;0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;0;map;8.0;9.0;10.0;11.0;12.0;13.0;14.0" />
+        </BehaviorTree>
+      </root>)";
+
+  BT::BehaviorTreeFactory factory;
+  factory.registerNodeType<TestNode<nav_msgs::msg::Path>>(
+    "PathPortTest");
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  tree = factory.createTreeFromText(xml_txt);
+  nav_msgs::msg::Path path;
+  tree.rootNode()->getInput("test", path);
+  EXPECT_EQ(rclcpp::Time(path.header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(path.header.frame_id, "map");
+  EXPECT_EQ(rclcpp::Time(path.poses[0].header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(path.poses[0].header.frame_id, "map");
+  EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
+  EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
+  EXPECT_EQ(path.poses[0].pose.position.z, 3.0);
+  EXPECT_EQ(path.poses[0].pose.orientation.x, 4.0);
+  EXPECT_EQ(path.poses[0].pose.orientation.y, 5.0);
+  EXPECT_EQ(path.poses[0].pose.orientation.z, 6.0);
+  EXPECT_EQ(path.poses[0].pose.orientation.w, 7.0);
+  EXPECT_EQ(rclcpp::Time(path.poses[1].header.stamp).nanoseconds(), 0);
+  EXPECT_EQ(path.poses[1].header.frame_id, "map");
+  EXPECT_EQ(path.poses[1].pose.position.x, 8.0);
+  EXPECT_EQ(path.poses[1].pose.position.y, 9.0);
+  EXPECT_EQ(path.poses[1].pose.position.z, 10.0);
+  EXPECT_EQ(path.poses[1].pose.orientation.x, 11.0);
+  EXPECT_EQ(path.poses[1].pose.orientation.y, 12.0);
+  EXPECT_EQ(path.poses[1].pose.orientation.z, 13.0);
+  EXPECT_EQ(path.poses[1].pose.orientation.w, 14.0);
+}
+
 TEST(MillisecondsPortTest, test_correct_syntax)
 {
   std::string xml_txt =