fix

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

Author: tonynajjar

nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp

@@ -71,21 +71,19 @@ inline BT::NodeStatus GoalUpdater::tick()
   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_;
-    } 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());
-    }
+  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);
 
-  if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) {
+  if (!last_goals_received_.poses.empty()) {
     auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp);
     rclcpp::Time most_recent_goal_time =  rclcpp::Time(0, 0, node_->get_clock()->get_clock_type());
     for (const auto & g : goals) {
@@ -98,12 +96,12 @@ inline BT::NodeStatus GoalUpdater::tick()
     } else {
       RCLCPP_WARN(
         node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the "
-        "current goals (oldest: %f). Ignoring the received goals.",
+        "current goals (oldest current goal: %f). Ignoring the received goals.",
         last_goals_received_time.seconds(), most_recent_goal_time.seconds());
     }
+    setOutput("output_goals", goals);
   }
 
-  setOutput("output_goals", goals);
   return child_node_->executeTick();
 }