quasi-robotics
diff --git a/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion b/‎nav2_amcl/package.xml‎
Lines changed: 1 addition & 1 deletion
diff --git a/‎nav2_behavior_tree/CMakeLists.txt‎
Lines changed: 4 additions & 0 deletions b/‎nav2_behavior_tree/CMakeLists.txt‎
Lines changed: 4 additions & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp‎
Lines changed: 1 addition & 3 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp‎
Lines changed: 1 addition & 3 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎
Lines changed: 3 additions & 3 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp‎
Lines changed: 3 additions & 3 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp‎
Lines changed: 1 addition & 3 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp‎
Lines changed: 1 addition & 3 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp‎
Lines changed: 1 addition & 3 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp‎
Lines changed: 1 addition & 3 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp‎
Lines changed: 66 additions & 0 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp‎
Lines changed: 66 additions & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp‎
Lines changed: 1 addition & 0 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp‎
Lines changed: 1 addition & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp‎
Lines changed: 1 addition & 0 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp‎
Lines changed: 1 addition & 0 deletions
diff --git a/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp‎
Lines changed: 1 addition & 0 deletions b/‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp‎
Lines changed: 1 addition & 0 deletions
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.3.2</version>
+  <version>1.3.3</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in
 
@@ -246,6 +246,10 @@ install(DIRECTORY test/utils/
         DESTINATION include/${PROJECT_NAME}/utils/
 )
 
+install(DIRECTORY test/utils/
+  DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils
+)
+
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
 install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})
 
 
@@ -58,9 +58,7 @@ class BtActionNode : public BT::ActionNodeBase
     // Get the required items from the blackboard
     auto bt_loop_duration =
       config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
 
@@ -310,18 +310,18 @@ void BtActionServer<ActionT>::executeCallback()
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:
-      RCLCPP_INFO(logger_, "Goal succeeded");
       action_server_->succeeded_current(result);
+      RCLCPP_INFO(logger_, "Goal succeeded");
       break;
 
     case nav2_behavior_tree::BtStatus::FAILED:
-      RCLCPP_ERROR(logger_, "Goal failed");
       action_server_->terminate_current(result);
+      RCLCPP_ERROR(logger_, "Goal failed");
       break;
 
     case nav2_behavior_tree::BtStatus::CANCELED:
-      RCLCPP_INFO(logger_, "Goal canceled");
       action_server_->terminate_all(result);
+      RCLCPP_INFO(logger_, "Goal canceled");
       break;
   }
 
 
@@ -56,9 +56,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
     callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
 
@@ -59,9 +59,7 @@ class BtServiceNode : public BT::ActionNodeBase
     // Get the required items from the blackboard
     auto bt_loop_duration =
       config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
 
@@ -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
 
@@ -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
 {
 
@@ -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
 {
 
@@ -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
Original file line number	Diff line number	Diff line change
`@@ -246,6 +246,10 @@ install(DIRECTORY test/utils/`
`246`	`246`	`DESTINATION include/${PROJECT_NAME}/utils/`
`247`	`247`	`)`
`248`	`248`
	`249`	`+install(DIRECTORY test/utils/`
	`250`	`+ DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils`
	`251`	`+)`
	`252`	`+`
`249`	`253`	`install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})`
`250`	`254`	`install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})`
`251`	`255`
Original file line number	Diff line number	Diff line change
`@@ -23,6 +23,7 @@`
`23`	`23`	`#include "nav2_util/geometry_utils.hpp"`
`24`	`24`	`#include "nav2_util/robot_utils.hpp"`
`25`	`25`	`#include "behaviortree_cpp/action_node.h"`
	`26`	`+#include "nav2_behavior_tree/bt_utils.hpp"`
`26`	`27`
`27`	`28`	`namespace nav2_behavior_tree`
`28`	`29`	`{`
Original file line number	Diff line number	Diff line change
`@@ -24,6 +24,7 @@`
`24`	`24`	`#include "rclcpp/rclcpp.hpp"`
`25`	`25`	`#include "geometry_msgs/msg/pose_stamped.hpp"`
`26`	`26`	`#include "tf2_ros/buffer.h"`
	`27`	`+#include "nav2_behavior_tree/bt_utils.hpp"`
`27`	`28`
`28`	`29`	`namespace nav2_behavior_tree`
`29`	`30`	`{`