|
17 | 17 |
|
18 | 18 | #include <string> |
19 | 19 | #include <set> |
| 20 | +#include <vector> |
20 | 21 |
|
21 | 22 | #include "rclcpp/time.hpp" |
22 | 23 | #include "rclcpp/node.hpp" |
23 | 24 | #include "behaviortree_cpp/behavior_tree.h" |
24 | 25 | #include "geometry_msgs/msg/point.hpp" |
25 | 26 | #include "geometry_msgs/msg/quaternion.hpp" |
26 | 27 | #include "geometry_msgs/msg/pose_stamped.hpp" |
| 28 | +#include "nav_msgs/msg/path.hpp" |
27 | 29 |
|
28 | 30 | namespace BT |
29 | 31 | { |
@@ -102,6 +104,70 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) |
102 | 104 | } |
103 | 105 | } |
104 | 106 |
|
| 107 | +/** |
| 108 | + * @brief Parse XML string to std::vector<geometry_msgs::msg::PoseStamped> |
| 109 | + * @param key XML string |
| 110 | + * @return std::vector<geometry_msgs::msg::PoseStamped> |
| 111 | + */ |
| 112 | +template<> |
| 113 | +inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key) |
| 114 | +{ |
| 115 | + // 9 real numbers separated by semicolons |
| 116 | + auto parts = BT::splitString(key, ';'); |
| 117 | + if (parts.size() % 9 != 0) { |
| 118 | + throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)"); |
| 119 | + } else { |
| 120 | + std::vector<geometry_msgs::msg::PoseStamped> poses; |
| 121 | + for (size_t i = 0; i < parts.size(); i += 9) { |
| 122 | + geometry_msgs::msg::PoseStamped pose_stamped; |
| 123 | + pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i])); |
| 124 | + pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]); |
| 125 | + pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]); |
| 126 | + pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]); |
| 127 | + pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]); |
| 128 | + pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]); |
| 129 | + pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]); |
| 130 | + pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]); |
| 131 | + pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]); |
| 132 | + poses.push_back(pose_stamped); |
| 133 | + } |
| 134 | + return poses; |
| 135 | + } |
| 136 | +} |
| 137 | + |
| 138 | +/** |
| 139 | + * @brief Parse XML string to nav_msgs::msg::Path |
| 140 | + * @param key XML string |
| 141 | + * @return nav_msgs::msg::Path |
| 142 | + */ |
| 143 | +template<> |
| 144 | +inline nav_msgs::msg::Path convertFromString(const StringView key) |
| 145 | +{ |
| 146 | + // 9 real numbers separated by semicolons |
| 147 | + auto parts = BT::splitString(key, ';'); |
| 148 | + if ((parts.size() - 2) % 9 != 0) { |
| 149 | + throw std::runtime_error("invalid number of fields for Path attribute)"); |
| 150 | + } else { |
| 151 | + nav_msgs::msg::Path path; |
| 152 | + path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0])); |
| 153 | + path.header.frame_id = BT::convertFromString<std::string>(parts[1]); |
| 154 | + for (size_t i = 2; i < parts.size(); i += 9) { |
| 155 | + geometry_msgs::msg::PoseStamped pose_stamped; |
| 156 | + path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i])); |
| 157 | + pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]); |
| 158 | + pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]); |
| 159 | + pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]); |
| 160 | + pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]); |
| 161 | + pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]); |
| 162 | + pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]); |
| 163 | + pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]); |
| 164 | + pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]); |
| 165 | + path.poses.push_back(pose_stamped); |
| 166 | + } |
| 167 | + return path; |
| 168 | + } |
| 169 | +} |
| 170 | + |
105 | 171 | /** |
106 | 172 | * @brief Parse XML string to std::chrono::milliseconds |
107 | 173 | * @param key XML string |
|
0 commit comments