PoseStamped vector specialization (#4607)

* PoseStamped vector specialization Signed-off-by: Tony Najjar <[email protected]> * merge master Signed-off-by: Tony Najjar <[email protected]> * add path Signed-off-by: Tony Najjar <[email protected]> * fix size check Signed-off-by: Tony Najjar <[email protected]> * fix test Signed-off-by: Tony Najjar <[email protected]> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Sep 4, 2024 · 3ab6312 · 3ab6312
1 parent 34d41ed
commit 3ab6312
Show file tree

Hide file tree

Showing 23 changed files with 206 additions and 12 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp
@@ -17,13 +17,15 @@
 
 #include <string>
 #include <set>
+#include <vector>
 
 #include "rclcpp/time.hpp"
 #include "rclcpp/node.hpp"
 #include "behaviortree_cpp/behavior_tree.h"
 #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
 {
@@ -102,6 +104,70 @@ 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)
+{
+  // 9 real numbers separated by semicolons
+  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 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() - 2) % 9 != 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/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp
@@ -23,6 +23,7 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "behaviortree_cpp/action_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/...ehavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp
@@ -24,6 +24,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "tf2_ros/buffer.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/...ior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp
@@ -22,6 +22,7 @@
 
 #include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 
 namespace nav2_behavior_tree

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp
@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
 #include "tf2_ros/buffer.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp
@@ -20,6 +20,7 @@
 
 #include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/...ior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/...ior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp
@@ -17,6 +17,7 @@
 
 #include <string>
 #include "behaviortree_cpp/behavior_tree.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/...avior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp
@@ -20,6 +20,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
@@ -23,6 +23,7 @@
 #include "tf2_ros/buffer.h"
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp
@@ -23,6 +23,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp
@@ -25,6 +25,7 @@
 #include "nav2_util/odometry_utils.hpp"
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
@@ -20,7 +20,6 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp
@@ -20,7 +20,6 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp
@@ -16,7 +16,6 @@
 #include <string>
 
 #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
@@ -20,7 +20,6 @@
 #include "nav2_util/node_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp
@@ -15,7 +15,6 @@
 #include <string>
 #include <vector>
 #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp b/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp
@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
@@ -26,7 +26,6 @@
 #include "behaviortree_cpp/decorator_node.h"
 
 #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp
@@ -19,7 +19,6 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "behaviortree_cpp/decorator_node.h"
 #include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp
@@ -19,7 +19,6 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

diff --git a/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp b/nav2_behavior_tree/test/plugins/condition/test_path_expiring_timer.cpp
@@ -23,7 +23,6 @@
 
 #include "utils/test_behavior_tree_fixture.hpp"
 #include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 using namespace std::chrono;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT

diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp
@@ -36,8 +36,6 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree
     poses1.push_back(goal1);
     config_->blackboard->set("goal", goal1);
     config_->blackboard->set("goals", poses1);
-    config_->input_ports["goals"] = "";
-    config_->input_ports["goal"] = "";
     bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
       "goal_updated_controller", *config_);
     dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();

diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp
@@ -194,6 +194,136 @@ 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(PathPortTest, test_wrong_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" />
+        </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;0;map;1.0;2.0;3.0;4.0;5.0;6.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 =