diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 7075bc63f4..91e3e241b9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/time.hpp" #include "rclcpp/node.hpp" @@ -24,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 { @@ -102,6 +104,70 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } } +/** + * @brief Parse XML string to std::vector + * @param key XML string + * @return std::vector + */ +template<> +inline std::vector 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 attribute)"); + } else { + std::vector 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(parts[i])); + pose_stamped.header.frame_id = BT::convertFromString(parts[i + 1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[i + 2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[i + 3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[i + 4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[i + 5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); + pose_stamped.pose.orientation.w = BT::convertFromString(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(parts[0])); + path.header.frame_id = BT::convertFromString(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(parts[i])); + pose_stamped.header.frame_id = BT::convertFromString(parts[i + 1]); + pose_stamped.pose.position.x = BT::convertFromString(parts[i + 2]); + pose_stamped.pose.position.y = BT::convertFromString(parts[i + 3]); + pose_stamped.pose.position.z = BT::convertFromString(parts[i + 4]); + pose_stamped.pose.orientation.x = BT::convertFromString(parts[i + 5]); + pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); + pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); + pose_stamped.pose.orientation.w = BT::convertFromString(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 index 344afd546d..33552a24f0 100644 --- 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/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 67747c62b8..77a80728dd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_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/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 7e3e92e799..12344a5d3f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_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 index b79fabe2f9..44e582c5f5 100644 --- 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 index 89d4b7a573..22893e33ee 100644 --- 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/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 548c15268b..9958004bdf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -17,6 +17,7 @@ #include #include "behaviortree_cpp/behavior_tree.h" +#include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp index 8871892949..5e770b4bbd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp +++ b/nav2_behavior_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 index 7fbda19c63..38cf3369b0 100644 --- 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 index bdd4171185..63a7f60655 100644 --- 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 index ed454c0aa1..59a9fc5521 100644 --- 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 index 1b0e449431..665bf81bc3 100644 --- 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 index 7db1817c65..991e0ab7cf 100644 --- 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 index dbd84d8b2e..1e1e557e5f 100644 --- 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 #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 index 7024356203..e93ba5cc36 100644 --- 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 index 88d329efc2..962eef70dd 100644 --- 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 #include #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 index 9d93022912..02a778a72e 100644 --- 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 index 7f87695416..4d0f6fefef 100644 --- 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 index d0de920545..b2f235dbd1 100644 --- 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 index b8e5b3915a..87c9eb5bf1 100644 --- 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 index 92834a15f2..e21ce307da 100644 --- 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 index 2e5d34ed4f..4259e87a53 100644 --- 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( "goal_updated_controller", *config_); dummy_node_ = std::make_shared(); diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index d495587955..008e0a36b7 100644 --- 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"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>>( + "PoseStampedVectorPortTest"); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + + xml_txt = + R"( + + + + + )"; + + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); +} + +TEST(PoseStampedVectorPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>>( + "PoseStampedVectorPortTest"); + auto tree = factory.createTreeFromText(xml_txt); + + tree = factory.createTreeFromText(xml_txt); + std::vector 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"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "PathPortTest"); + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); + + xml_txt = + R"( + + + + + )"; + + EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); +} + +TEST(PathPortTest, test_correct_syntax) +{ + std::string xml_txt = + R"( + + + + + )"; + + BT::BehaviorTreeFactory factory; + factory.registerNodeType>( + "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 =