fixes

Signed-off-by: Tony Najjar <[email protected]>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp

@@ -38,6 +38,7 @@ namespace nav2_behavior_tree
 class GoalUpdater : public BT::DecoratorNode
 {
 public:
+  typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
   /**
    * @brief A constructor for nav2_behavior_tree::GoalUpdater
    * @param xml_tag_name Name for the XML tag for this node
@@ -55,10 +56,10 @@ class GoalUpdater : public BT::DecoratorNode
   {
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
-      BT::InputPort<nav2_msgs::msg::PosesStamped>("input_goals", "Original Goals"),
+      BT::InputPort<Goals>("input_goals", "Original Goals"),
       BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
           "Received Goal by subscription"),
-      BT::OutputPort<nav2_msgs::msg::PosesStamped>("output_goals", "Received Goals by subscription")
+      BT::OutputPort<Goals>("output_goals", "Received Goals by subscription")
     };
   }
 
@@ -85,7 +86,7 @@ class GoalUpdater : public BT::DecoratorNode
   rclcpp::Subscription<nav2_msgs::msg::PosesStamped>::SharedPtr goals_sub_;
 
   geometry_msgs::msg::PoseStamped last_goal_received_;
-  nav2_msgs::msg::PosesStamped last_goals_received_;
+  Goals last_goals_received_;
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -62,7 +62,7 @@ GoalUpdater::GoalUpdater(
 inline BT::NodeStatus GoalUpdater::tick()
 {
   geometry_msgs::msg::PoseStamped goal;
-  nav2_msgs::msg::PosesStamped goals;
+  Goals goals;
 
   getInput("input_goal", goal);
   getInput("input_goals", goals);
@@ -84,7 +84,7 @@ inline BT::NodeStatus GoalUpdater::tick()
     }
   }
 
-  if (!last_goals_received_.poses.empty()) {
+  if (!last_goals_received_.empty()) {
     goals = last_goals_received_;
   }
 
@@ -104,7 +104,7 @@ void
 GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  last_goals_received_ = *msg;
+  last_goals_received_ = msg->poses;
 }
 
 }  // namespace nav2_behavior_tree