Use PoseStampedArray (#4791)

* Add IsStoppedBTNode

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add topic name + reformat

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix comment

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix abs

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove log

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add getter functions for raw twist

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* remove unused code

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use odomsmoother

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* update groot

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add test

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* reset at success

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* FIX velocity_threshold_

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix stopped Node

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Add tests  to odometry_utils

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix linting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Use PoseStampedArray

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* use geometry_msgs

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* more fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* .

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* revert

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* .

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* update

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix bt_utils

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix import

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* populate

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

@@ -23,8 +23,9 @@
 #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 "geometry_msgs/msg/pose_stamped_array.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
 #include "nav_msgs/msg/path.hpp"
 
 namespace BT
@@ -112,7 +113,6 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
 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)");
@@ -135,6 +135,38 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
   }
 }
 
+/**
+ * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray
+ * @param key XML string
+ * @return geometry_msgs::msg::PoseStampedArray
+ */
+template<>
+inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key)
+{
+  auto parts = BT::splitString(key, ';');
+  if ((parts.size() - 2) % 9 != 0) {
+    throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)");
+  } else {
+    geometry_msgs::msg::PoseStampedArray pose_stamped_array;
+    pose_stamped_array.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    pose_stamped_array.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;
+      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]);
+      pose_stamped_array.poses.push_back(pose_stamped);
+    }
+    return pose_stamped_array;
+  }
+}
+
 /**
  * @brief Parse XML string to nav_msgs::msg::Path
  * @param key XML string
@@ -143,7 +175,6 @@ inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const Stri
 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)");

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

@@ -75,7 +75,7 @@ class ComputePathThroughPosesAction
   {
     return providedBasicPorts(
       {
-        BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
           "goals",
           "Destinations to plan through"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp

@@ -18,8 +18,7 @@
 #include <string>
 #include <vector>
 
-#include "geometry_msgs/msg/point.hpp"
-#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_msgs/action/navigate_through_poses.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
@@ -74,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
   {
     return providedBasicPorts(
       {
-        BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+        BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
           "goals", "Destinations to plan through"),
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
         BT::OutputPort<ActionResult::_error_code_type>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -20,7 +20,7 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_behavior_tree/bt_service_node.hpp"
 #include "nav2_msgs/srv/get_costs.hpp"
 
@@ -30,8 +30,6 @@ namespace nav2_behavior_tree
 class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 {
 public:
-  typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
-
   /**
    * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals
    * @param service_node_name Service name this node creates a client for
@@ -54,23 +52,25 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
   {
     return providedBasicPorts(
       {
-        BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
+        BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
+          "Original goals to remove from"),
         BT::InputPort<double>(
           "cost_threshold", 254.0,
           "Cost threshold for considering a goal in collision"),
         BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
         BT::InputPort<bool>(
           "consider_unknown_as_obstacle", false,
           "Whether to consider unknown cost as obstacle"),
-        BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
+        BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
+          "Goals with in-collision goals removed"),
       });
   }
 
 private:
   bool use_footprint_;
   bool consider_unknown_as_obstacle_;
   double cost_threshold_;
-  Goals input_goals_;
+  geometry_msgs::msg::PoseStampedArray input_goals_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -19,7 +19,7 @@
 #include <memory>
 #include <string>
 
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "behaviortree_cpp/action_node.h"
@@ -31,8 +31,6 @@ namespace nav2_behavior_tree
 class RemovePassedGoals : public BT::ActionNodeBase
 {
 public:
-  typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
-
   RemovePassedGoals(
     const std::string & xml_tag_name,
     const BT::NodeConfiguration & conf);
@@ -45,8 +43,10 @@ class RemovePassedGoals : public BT::ActionNodeBase
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<Goals>("input_goals", "Original goals to remove viapoints from"),
-      BT::OutputPort<Goals>("output_goals", "Goals with passed viapoints removed"),
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals",
+          "Original goals to remove viapoints from"),
+      BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
+          "Goals with passed viapoints removed"),
       BT::InputPort<double>("radius", 0.5, "radius to goal for it to be considered for removal"),
       BT::InputPort<std::string>("global_frame", "Global frame"),
       BT::InputPort<std::string>("robot_base_frame", "Robot base frame"),

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "behaviortree_cpp/condition_node.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
 
 
@@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
         "goals", "Vector of navigation goals"),
       BT::InputPort<geometry_msgs::msg::PoseStamped>(
         "goal", "Navigation goal"),
@@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode
   bool first_time;
   rclcpp::Node::SharedPtr node_;
   geometry_msgs::msg::PoseStamped goal_;
-  std::vector<geometry_msgs::msg::PoseStamped> goals_;
+  geometry_msgs::msg::PoseStampedArray goals_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

@@ -19,7 +19,7 @@
 #include <vector>
 
 #include "behaviortree_cpp/condition_node.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
@@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
         "goals", "Vector of navigation goals"),
       BT::InputPort<geometry_msgs::msg::PoseStamped>(
         "goal", "Navigation goal"),
@@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode
 
 private:
   geometry_msgs::msg::PoseStamped goal_;
-  std::vector<geometry_msgs::msg::PoseStamped> goals_;
+  geometry_msgs::msg::PoseStampedArray goals_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp

@@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
         "goals", "Vector of navigation goals"),
       BT::InputPort<geometry_msgs::msg::PoseStamped>(
         "goal", "Navigation goal"),
@@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode
 
   bool goal_was_updated_;
   geometry_msgs::msg::PoseStamped goal_;
-  std::vector<geometry_msgs::msg::PoseStamped> goals_;
+  geometry_msgs::msg::PoseStampedArray goals_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

@@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode
       BT::InputPort<double>("max_rate", 1.0, "Maximum rate"),
       BT::InputPort<double>("min_speed", 0.0, "Minimum speed"),
       BT::InputPort<double>("max_speed", 0.5, "Maximum speed"),
-      BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>(
         "goals", "Vector of navigation goals"),
       BT::InputPort<geometry_msgs::msg::PoseStamped>(
         "goal", "Navigation goal"),
@@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode
 
   // current goal
   geometry_msgs::msg::PoseStamped goal_;
-  std::vector<geometry_msgs::msg::PoseStamped> goals_;
+  geometry_msgs::msg::PoseStampedArray goals_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -39,15 +39,15 @@ void RemoveInCollisionGoals::on_tick()
   getInput("input_goals", input_goals_);
   getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 
-  if (input_goals_.empty()) {
+  if (input_goals_.poses.empty()) {
     setOutput("output_goals", input_goals_);
     should_send_request_ = false;
     return;
   }
   request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
   request_->use_footprint = use_footprint_;
 
-  for (const auto & goal : input_goals_) {
+  for (const auto & goal : input_goals_.poses) {
     request_->poses.push_back(goal);
   }
 }
@@ -63,16 +63,16 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
     return BT::NodeStatus::FAILURE;
   }
 
-  Goals valid_goal_poses;
+  geometry_msgs::msg::PoseStampedArray valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
       response->costs[i] < cost_threshold_)
     {
-      valid_goal_poses.push_back(input_goals_[i]);
+      valid_goal_poses.poses.push_back(input_goals_.poses[i]);
     }
   }
   // Inform if all goals have been removed
-  if (valid_goal_poses.empty()) {
+  if (valid_goal_poses.poses.empty()) {
     RCLCPP_INFO(
       node_->get_logger(),
       "All goals are in collision and have been removed from the list");

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick()
     initialize();
   }
 
-  Goals goal_poses;
+  geometry_msgs::msg::PoseStampedArray goal_poses;
   getInput("input_goals", goal_poses);
 
-  if (goal_poses.empty()) {
+  if (goal_poses.poses.empty()) {
     setOutput("output_goals", goal_poses);
     return BT::NodeStatus::SUCCESS;
   }
@@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick()
 
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_,
+      current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_,
       transform_tolerance_))
   {
     return BT::NodeStatus::FAILURE;
   }
 
   double dist_to_goal;
-  while (goal_poses.size() > 1) {
-    dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose);
+  while (goal_poses.poses.size() > 1) {
+    dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose);
 
     if (dist_to_goal > viapoint_achieved_radius_) {
       break;
     }
 
-    goal_poses.erase(goal_poses.begin());
+    goal_poses.poses.erase(goal_poses.poses.begin());
   }
 
   setOutput("output_goals", goal_poses);

nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

@@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick()
     return BT::NodeStatus::SUCCESS;
   }
 
-  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  geometry_msgs::msg::PoseStampedArray current_goals;
   BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
   BT::getInputOrBlackboard("goal", current_goal);

nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick()
     return BT::NodeStatus::FAILURE;
   }
 
-  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  geometry_msgs::msg::PoseStampedArray current_goals;
   geometry_msgs::msg::PoseStamped current_goal;
   BT::getInputOrBlackboard("goals", current_goals);
   BT::getInputOrBlackboard("goal", current_goal);

nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

@@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick()
 
   setStatus(BT::NodeStatus::RUNNING);
 
-  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  geometry_msgs::msg::PoseStampedArray current_goals;
   BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
   BT::getInputOrBlackboard("goal", current_goal);

nav2_behavior_tree/plugins/decorator/speed_controller.cpp

@@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick()
     first_tick_ = true;
   }
 
-  std::vector<geometry_msgs::msg::PoseStamped> current_goals;
+  geometry_msgs::msg::PoseStampedArray current_goals;
   BT::getInputOrBlackboard("goals", current_goals);
   geometry_msgs::msg::PoseStamped current_goal;
   BT::getInputOrBlackboard("goal", current_goal);