Fix Goal updater QoS (#3719)

* Fix GoalUpdater QoS

* Fixes

Author: Tony Najjar

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -46,7 +46,7 @@ GoalUpdater::GoalUpdater(
   sub_option.callback_group = callback_group_;
   goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
     goal_updater_topic,
-    10,
+    rclcpp::SystemDefaultsQoS(),
     std::bind(&GoalUpdater::callback_updated_goal, this, _1),
     sub_option);
 }
@@ -59,8 +59,17 @@ inline BT::NodeStatus GoalUpdater::tick()
 
   callback_group_executor_.spin_some();
 
-  if (rclcpp::Time(last_goal_received_.header.stamp) > rclcpp::Time(goal.header.stamp)) {
-    goal = last_goal_received_;
+  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_;
+    } else {
+      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());
+    }
   }
 
   setOutput("output_goal", goal);

nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

@@ -95,42 +95,101 @@ TEST_F(GoalUpdaterTestFixture, test_tick)
       </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;
   goal.header.stamp = node_->now();
   goal.pose.position.x = 1.0;
   config_->blackboard->set("goal", goal);
 
-  // tick until node succeeds
-  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
-    tree_->rootNode()->executeTick();
-  }
-
+  // tick tree without publishing updated goal and get updated_goal
+  tree_->rootNode()->executeTick();
   geometry_msgs::msg::PoseStamped updated_goal;
   config_->blackboard->get("updated_goal", updated_goal);
+}
 
-  EXPECT_EQ(updated_goal, goal);
+TEST_F(GoalUpdaterTestFixture, test_older_goal_update)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+            <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;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
 
+  // publish updated_goal older than goal
   geometry_msgs::msg::PoseStamped goal_to_update;
-  goal_to_update.header.stamp = node_->now();
+  goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0);
   goal_to_update.pose.position.x = 2.0;
 
+  goal_updater_pub->publish(goal_to_update);
+  tree_->rootNode()->executeTick();
+  geometry_msgs::msg::PoseStamped updated_goal;
+  config_->blackboard->get("updated_goal", updated_goal);
+
+  // expect to succeed and not update goal
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(updated_goal, goal);
+}
+
+TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
+            <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);
 
-  auto start = node_->now();
-  while ((node_->now() - start).seconds() < 0.5) {
-    tree_->rootNode()->executeTick();
-    goal_updater_pub->publish(goal_to_update);
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
 
-    rclcpp::spin_some(node_);
-  }
+  // publish updated_goal older than goal
+  geometry_msgs::msg::PoseStamped goal_to_update_1;
+  goal_to_update_1.header.stamp = node_->now();
+  goal_to_update_1.pose.position.x = 2.0;
+
+  geometry_msgs::msg::PoseStamped goal_to_update_2;
+  goal_to_update_2.header.stamp = node_->now();
+  goal_to_update_2.pose.position.x = 3.0;
 
+  goal_updater_pub->publish(goal_to_update_1);
+  goal_updater_pub->publish(goal_to_update_2);
+  tree_->rootNode()->executeTick();
+  geometry_msgs::msg::PoseStamped updated_goal;
   config_->blackboard->get("updated_goal", updated_goal);
 
-  EXPECT_NE(updated_goal, goal);
-  EXPECT_EQ(updated_goal, goal_to_update);
+  // 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);
 }
 
 int main(int argc, char ** argv)