Adapt GoalUpdater to update goals as well

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

@@ -1,5 +1,6 @@
 // Copyright (c) 2018 Intel Corporation
 // Copyright (c) 2020 Francisco Martin Rico
+// Copyright (c) 2024 Angsa Robotics
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -20,6 +21,7 @@
 #include <string>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_stamped_array.hpp"
 
 #include "behaviortree_cpp/decorator_node.h"
 
@@ -52,9 +54,11 @@ class GoalUpdater : public BT::DecoratorNode
   {
     return {
       BT::InputPort<geometry_msgs::msg::PoseStamped>("input_goal", "Original Goal"),
-      BT::OutputPort<geometry_msgs::msg::PoseStamped>(
-        "output_goal",
-        "Received Goal by subscription"),
+      BT::InputPort<geometry_msgs::msg::PoseStampedArray>("input_goals", "Original Goals"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>("output_goal",
+          "Received Goal by subscription"),
+      BT::OutputPort<geometry_msgs::msg::PoseStampedArray>("output_goals",
+          "Received Goals by subscription")
     };
   }
 
@@ -71,9 +75,19 @@ class GoalUpdater : public BT::DecoratorNode
    */
   void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
 
+  /**
+   * @brief Callback function for goals update topic
+   * @param msg Shared pointer to geometry_msgs::msg::PoseStampedArray message
+   */
+  void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg);
+
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStampedArray>::SharedPtr goals_sub_;
 
   geometry_msgs::msg::PoseStamped last_goal_received_;
+  bool last_goal_received_set_{false};
+  geometry_msgs::msg::PoseStampedArray last_goals_received_;
+  bool last_goals_received_set_{false};
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -328,7 +328,9 @@
 
     <Decorator ID="GoalUpdater">
       <input_port name="input_goal">Original goal in</input_port>
+      <input_port name="input_goals">Original goals in</input_port>
       <output_port name="output_goal">Output goal set by subscription</output_port>
+      <output_port name="output_goals">Output goals set by subscription</output_port>
     </Decorator>
 
     <Decorator ID="SpeedController">

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -1,5 +1,6 @@
 // Copyright (c) 2018 Intel Corporation
 // Copyright (c) 2020 Francisco Martin Rico
+// Copyright (c) 2024 Angsa Robotics
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -16,7 +17,6 @@
 #include <string>
 #include <memory>
 
-#include "geometry_msgs/msg/pose_stamped.hpp"
 #include "behaviortree_cpp/decorator_node.h"
 
 #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
@@ -40,7 +40,9 @@ GoalUpdater::GoalUpdater(
   callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
   std::string goal_updater_topic;
+  std::string goals_updater_topic;
   node_->get_parameter_or<std::string>("goal_updater_topic", goal_updater_topic, "goal_update");
+  node_->get_parameter_or<std::string>("goals_updater_topic", goals_updater_topic, "goals_update");
 
   rclcpp::SubscriptionOptions sub_option;
   sub_option.callback_group = callback_group_;
@@ -49,39 +51,87 @@ GoalUpdater::GoalUpdater(
     rclcpp::SystemDefaultsQoS(),
     std::bind(&GoalUpdater::callback_updated_goal, this, _1),
     sub_option);
+  goals_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStampedArray>(
+    goals_updater_topic,
+    rclcpp::SystemDefaultsQoS(),
+    std::bind(&GoalUpdater::callback_updated_goals, this, _1),
+    sub_option);
 }
 
 inline BT::NodeStatus GoalUpdater::tick()
 {
   geometry_msgs::msg::PoseStamped goal;
+  geometry_msgs::msg::PoseStampedArray goals;
 
   getInput("input_goal", goal);
+  getInput("input_goals", goals);
 
   // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin
   callback_group_executor_.spin_all(std::chrono::milliseconds(1));
   callback_group_executor_.spin_all(std::chrono::milliseconds(49));
 
-  if (last_goal_received_.header.stamp != rclcpp::Time(0)) {
-    auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
-    auto goal_time = rclcpp::Time(goal.header.stamp);
-    if (last_goal_received_time > goal_time) {
-      goal = last_goal_received_;
+  if (last_goal_received_set_) {
+    if (last_goal_received_.header.stamp == rclcpp::Time(0)) {
+        // if the goal doesn't have a timestamp, we reject it
+      RCLCPP_WARN(
+          node_->get_logger(), "The received goal has no timestamp. Ignoring.");
+      setOutput("output_goal", goal);
     } else {
+      auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp);
+      auto goal_time = rclcpp::Time(goal.header.stamp);
+      if (last_goal_received_time >= goal_time) {
+        setOutput("output_goal", last_goal_received_);
+      } else {
+        RCLCPP_INFO(
+            node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
+            "current goal (%f). Ignoring the received goal.",
+            last_goal_received_time.seconds(), goal_time.seconds());
+        setOutput("output_goal", goal);
+      }
+    }
+  } else {
+    setOutput("output_goal", goal);
+  }
+
+  if (last_goals_received_set_) {
+    if (last_goals_received_.poses.empty()) {
+      setOutput("output_goals", goals);
+    } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) {
       RCLCPP_WARN(
-        node_->get_logger(), "The timestamp of the received goal (%f) is older than the "
-        "current goal (%f). Ignoring the received goal.",
-        last_goal_received_time.seconds(), goal_time.seconds());
+          node_->get_logger(), "The received goals array has no timestamp. Ignoring.");
+      setOutput("output_goals", goals);
+    } else {
+      auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp);
+      auto goals_time = rclcpp::Time(goals.header.stamp);
+      if (last_goals_received_time >= goals_time) {
+        setOutput("output_goals", last_goals_received_);
+      } else {
+        RCLCPP_INFO(
+            node_->get_logger(), "The timestamp of the received goals (%f) is older than the "
+            "current goals (%f). Ignoring the received goals.",
+            last_goals_received_time.seconds(), goals_time.seconds());
+        setOutput("output_goals", goals);
+      }
     }
+  } else {
+    setOutput("output_goals", goals);
   }
 
-  setOutput("output_goal", goal);
   return child_node_->executeTick();
 }
 
 void
 GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
 {
   last_goal_received_ = *msg;
+  last_goal_received_set_ = true;
+}
+
+void
+GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg)
+{
+  last_goals_received_ = *msg;
+  last_goals_received_set_ = true;
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

@@ -88,26 +88,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+          <GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
             <AlwaysSuccess/>
           </GoalUpdater>
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
-  auto goal_updater_pub =
-    node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
 
   // create new goal and set it on blackboard
   geometry_msgs::msg::PoseStamped goal;
+  geometry_msgs::msg::PoseStampedArray goals;
   goal.header.stamp = node_->now();
   goal.pose.position.x = 1.0;
+  goals.poses.push_back(goal);
   config_->blackboard->set("goal", goal);
+  config_->blackboard->set("goals", goals);
 
   // tick tree without publishing updated goal and get updated_goal
   tree_->rootNode()->executeTick();
   geometry_msgs::msg::PoseStamped updated_goal;
+  geometry_msgs::msg::PoseStampedArray updated_goals;
   EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
+  EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
 }
 
 TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
@@ -117,7 +120,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+          <GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
             <AlwaysSuccess/>
           </GoalUpdater>
         </BehaviorTree>
@@ -126,26 +129,39 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   auto goal_updater_pub =
     node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
+  auto goals_updater_pub =
+    node_->create_publisher<geometry_msgs::msg::PoseStampedArray>("goals_update", 10);
 
   // create new goal and set it on blackboard
   geometry_msgs::msg::PoseStamped goal;
+  geometry_msgs::msg::PoseStampedArray goals;
   goal.header.stamp = node_->now();
   goal.pose.position.x = 1.0;
+  goals.header.stamp = goal.header.stamp;
+  goals.poses.push_back(goal);
   config_->blackboard->set("goal", goal);
+  config_->blackboard->set("goals", goals);
 
   // publish updated_goal older than goal
   geometry_msgs::msg::PoseStamped goal_to_update;
+  geometry_msgs::msg::PoseStampedArray goals_to_update;
   goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0);
   goal_to_update.pose.position.x = 2.0;
+  goals_to_update.header.stamp = goal_to_update.header.stamp;
+  goals_to_update.poses.push_back(goal_to_update);
 
   goal_updater_pub->publish(goal_to_update);
+  goals_updater_pub->publish(goals_to_update);
   tree_->rootNode()->executeTick();
   geometry_msgs::msg::PoseStamped updated_goal;
+  geometry_msgs::msg::PoseStampedArray updated_goals;
   EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
+  EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
 
   // expect to succeed and not update goal
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
   EXPECT_EQ(updated_goal, goal);
+  EXPECT_EQ(updated_goals, goals);
 }
 
 TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
@@ -155,7 +171,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+          <GoalUpdater input_goal="{goal}" input_goals="{goals}" output_goal="{updated_goal}" output_goals="{updated_goals}">
             <AlwaysSuccess/>
           </GoalUpdater>
         </BehaviorTree>
@@ -164,32 +180,48 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   auto goal_updater_pub =
     node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
+  auto goals_updater_pub =
+    node_->create_publisher<geometry_msgs::msg::PoseStampedArray>("goals_update", 10);
 
   // create new goal and set it on blackboard
   geometry_msgs::msg::PoseStamped goal;
+  geometry_msgs::msg::PoseStampedArray goals;
   goal.header.stamp = node_->now();
   goal.pose.position.x = 1.0;
+  goals.poses.push_back(goal);
   config_->blackboard->set("goal", goal);
+  config_->blackboard->set("goals", goals);
 
   // publish updated_goal older than goal
   geometry_msgs::msg::PoseStamped goal_to_update_1;
+  geometry_msgs::msg::PoseStampedArray goals_to_update_1;
   goal_to_update_1.header.stamp = node_->now();
   goal_to_update_1.pose.position.x = 2.0;
+  goals_to_update_1.header.stamp = goal_to_update_1.header.stamp;
+  goals_to_update_1.poses.push_back(goal_to_update_1);
 
   geometry_msgs::msg::PoseStamped goal_to_update_2;
+  geometry_msgs::msg::PoseStampedArray goals_to_update_2;
   goal_to_update_2.header.stamp = node_->now();
   goal_to_update_2.pose.position.x = 3.0;
+  goals_to_update_2.header.stamp = goal_to_update_2.header.stamp;
+  goals_to_update_2.poses.push_back(goal_to_update_2);
 
   goal_updater_pub->publish(goal_to_update_1);
+  goals_updater_pub->publish(goals_to_update_1);
   goal_updater_pub->publish(goal_to_update_2);
+  goals_updater_pub->publish(goals_to_update_2);
   tree_->rootNode()->executeTick();
   geometry_msgs::msg::PoseStamped updated_goal;
+  geometry_msgs::msg::PoseStampedArray updated_goals;
   EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal));
+  EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals));
 
   // expect to succeed
   EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
   // expect to update goal with latest goal update
   EXPECT_EQ(updated_goal, goal_to_update_2);
+  EXPECT_EQ(updated_goals, goals_to_update_2);
 }
 
 int main(int argc, char ** argv)