Iron sync 2, Aug 4, 1.2.2 (#3740)

* Only apply Wnon-virtual-dtor if the compile language is CXX (#3614)

* https://stackoverflow.com/questions/25525047/cmake-generator-expression-differentiate-c-c-code

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix map not showing on rviz when navigation is launched with namespace (#3620)

* Fix Wshadow errors and enforce it (#3617)

* Fix Wshadow errors and enforce it

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Remove workaround for pluginlib

* This was only needed because it was included transitively
* By finding and linking properly, the compiler flags get propogated as SYSTEM
  correctly

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

---------

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* add-Wnull-dereference and fix warnings (#3622)

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* updating mppi's path angle critic for optional bidirectionality (#3624)

* updating mppi's path angle critic for optional bidirectionality

* Update README.md

* correct error message (#3631)

* correct error message

* clean up

* cleanup

* remove header

* Let Navigators have different error codes (#3642)

* Change ERROR to DEBUG

* INFO message on init

* format code

* Replace newlines with spaces

* fixing path angle critic's non-directional bias (#3632)

* fixing path angle critic's non-directional bias

* adding reformat

* adapting goal critic for speed to goal (#3641)

* adapting goal critic for speed to goal

* retuning goal critic

* add readme entries

* Update critics_tests.cpp

* Fix uninitialized value (#3651)

* In NAV2, this warning is treated as an error

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Fix rviz panel node arguments (#3655)

Signed-off-by: Nick Lamprianidis <info@nlamprian.me>

* Reduce out-of-range log to DEBUG (#3656)

* Adding nan twist rejection for velocity smoother and collision monitor (#3658)

* adding nan twist rejection for velocity smoother and collision monitor

* deref

* Ceres exposes a namespaced export and recommends it in their docs (#3652)

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

* Enable multiple MPPI path angle modes depending on preferences in behavior (#3650)

* fixing path angle critic's non-directional bias

* adding reformat

* handle linting

* add utility unit tests

* adding unit tests for path angle

* MPPI: Support Exact Path Following For Feasible Plans (#3659)

* alternative to path align critic for inversion control

* fix default behavior (enforce_path_inversion: false) (#3643)

Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* adding dyaw option for path alignment to incentivize following the path's intent where necessary

* add docs for use path orientations

* fix typo

---------

Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX (#3662)

* Resolves 3646: Update CMAKE_COMPILER_IS_GNUCXX

* Update CMakeLists.txt

* Fix smoother server tests (#3663)

* Fix smoother server tests

* Update test_smoother_server.cpp

* fix some new build warnings from sync (#3674)

* fix some new build warnings

* fixing last issue

* Update navigate_through_poses_action.hpp

* adding unsigned int to tests

* all to unsigned shorts

* test new warning resolution

* Update

* convert unsigned shorts to uint16_t for linter

* Fix costmap publisher test (#3679)

* added printouts

* ignore system tests

* fix

* cleanup

* Update test_costmap_2d_publisher.cpp

remove space

* remove empty message (#3691)

* Collision Monitor fixups (#3696)

* Fix max_points -> min_points in parameters
* Move robot_utils.hpp include to source where it actually using
* Remove double-description of getTransform()

* Use ParameterFile (allow_substs) (#3706)

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* nav2_bt_navigator: log current location on navigate_to_pose action initialization (#3720)

It is very useful to know the current location considered by the
bt_navigator for debug purposes.

* nav2_behaviors: export all available plugins (#3716)

It allows external packages to include those headers and create child
classes through inheritance.

* changing costmap layers private to protected (#3722)

* Update costmap_2d_ros.cpp (#3687)

* updated nav2_behavior_tree test util install path (#3718)

* launch linting (#3729)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (#3728)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially

* fix test failures

* Update RewrittenYaml to support list rewrites (#3727)

* allowing leaf key rewrites that aren't dcits (#3730)

* adding checks on config and dynamic parameters for proper velocity and acceleration limits (#3731)

* Fix Goal updater QoS (#3719)

* Fix GoalUpdater QoS

* Fixes

* adding tolerance back in for smac lattice and hybrid-A* planners (#3734)

* Completing Hybrid-A* visualization of expansion footprints PR (#3733)

* smach_planner_hybrid: add support visualization for hybrid Astar

* smac_planner_hyrid: revert some

* smach_planner_hybrid: improving code quality

* utils: add some useful functions

* utils: fix mistake

* nav2_smac_planner: fix format problem

* utils: fix format and revise functions

* smach_planner_hybrid: delete _viz_expansion parameter

* smac_planner_hybrid: fix format

* README: update parameter

* utils: corrct mistake return

* utils: make timestamp a const reference

* nav2_smac_planner: correct format problem

* add unit test functions

* further detection of element equality

* test_utils: add non-trival translation and rotation

* smac_planner_hybrid: pass value instead of references

* completing hybrid A* visualization

---------

Co-authored-by: xianglunkai <1322099375@qq.com>

* Update README.md (#3736)

* Update README.md

* Update README.md

* sync iron to 1.2.2 to release

---------

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
Signed-off-by: Nick Lamprianidis <info@nlamprian.me>
Signed-off-by: ymd-stella <world.applepie@gmail.com>
Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com>
Co-authored-by: Filipe Cerveira <filipecerveira@gmail.com>
Co-authored-by: Joshua Wallace <josho.wallace@gmail.com>
Co-authored-by: RBT22 <43723477+RBT22@users.noreply.github.com>
Co-authored-by: Nick Lamprianidis <info@nlamprian.me>
Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com>
Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com>
Co-authored-by: Aaditya Ravindran <aravindran@locusrobotics.com>
Co-authored-by: gyaanantia <78275262+gyaanantia@users.noreply.github.com>
Co-authored-by: Tony Najjar <tony.najjar@logivations.com>
Co-authored-by: xianglunkai <1322099375@qq.com>

README.md

@@ -55,6 +55,19 @@ please cite this work in your papers!
  }
 ```
 
+If you use **any** of the algorithms in Nav2 or the analysis of the algorithms in your work, please cite this work in your papers!
+
+- S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson, [**From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2**](https://arxiv.org/pdf/2307.15236.pdf), Robotics and Autonomous Systems, 2023.
+
+```bibtex
+  @article{macenski2023survey,
+        title={From the desks of ROS maintainers: A survey of modern & capable mobile robotics algorithms in the robot operating system 2}, 
+        author={S. Macenski, T. Moore, DV Lu, A. Merzlyakov, M. Ferguson},
+        year={2023},
+        journal = {Robotics and Autonomous Systems}
+  }
+```
+
 If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers!
 
 - S. Macenski, S. Singh, F. Martin, J. Gines, [**Regulated Pure Pursuit for Robot Path Tracking**](https://arxiv.org/abs/2305.20026). Autonomous Robots, 2023.

nav2_amcl/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

nav2_amcl/src/pf/eig3.c

@@ -43,7 +43,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
 //  Fortran subroutine in EISPACK.
 
   int i, j, k;
-  double f, g, h, hh;
+  double f, g, hh;
   for (j = 0; j < n; j++) {
     d[j] = V[n - 1][j];
   }
@@ -122,7 +122,7 @@ static void tred2(double V[n][n], double d[n], double e[n])
   for (i = 0; i < n - 1; i++) {
     V[n - 1][i] = V[i][i];
     V[i][i] = 1.0;
-    h = d[i + 1];
+    const double h = d[i + 1];
     if (h != 0.0) {
       for (k = 0; k <= i; k++) {
         d[k] = V[k][i + 1] / h;

nav2_behavior_tree/CMakeLists.txt

@@ -224,7 +224,7 @@ install(DIRECTORY include/
 )
 
 install(DIRECTORY test/utils/
-        DESTINATION include/utils/
+        DESTINATION include/${PROJECT_NAME}/utils/
 )
 
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -67,16 +67,29 @@ BtActionServer<ActionT>::BtActionServer(
   };
 
   if (!node->has_parameter("error_code_names")) {
-    std::string error_codes_str;
-    for (const auto & error_code : error_code_names) {
-      error_codes_str += error_code + "\n";
+    const rclcpp::ParameterValue value = node->declare_parameter(
+      "error_code_names",
+      rclcpp::PARAMETER_STRING_ARRAY);
+    if (value.get_type() == rclcpp::PARAMETER_NOT_SET) {
+      std::string error_codes_str;
+      for (const auto & error_code : error_code_names) {
+        error_codes_str += " " + error_code;
+      }
+      RCLCPP_WARN_STREAM(
+        logger_, "Error_code parameters were not set. Using default values of:"
+          << error_codes_str + "\n"
+          << "Make sure these match your BT and there are not other sources of error codes you"
+          "reported to your application");
+      rclcpp::Parameter error_code_names_param("error_code_names", error_code_names);
+      node->set_parameter(error_code_names_param);
+    } else {
+      error_code_names = value.get<std::vector<std::string>>();
+      std::string error_codes_str;
+      for (const auto & error_code : error_code_names) {
+        error_codes_str += " " + error_code;
+      }
+      RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str);
     }
-    RCLCPP_WARN_STREAM(
-      logger_, "Error_code parameters were not set. Using default values of: "
-        << error_codes_str
-        << "Make sure these match your BT and there are not other sources of error codes you want "
-        "reported to your application");
-    node->declare_parameter("error_code_names", error_code_names);
   }
 }
 
@@ -279,7 +292,7 @@ void BtActionServer<ActionT>::populateErrorCode(
         highest_priority_error_code = current_error_code;
       }
     } catch (...) {
-      RCLCPP_ERROR(
+      RCLCPP_DEBUG(
         logger_,
         "Failed to get error code: %s from blackboard",
         error_code.c_str());

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

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
 
 #include <string>
+#include <vector>
 
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
@@ -74,7 +75,8 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
   {
     return providedBasicPorts(
       {
-        BT::InputPort<geometry_msgs::msg::PoseStamped>("goals", "Destinations to plan through"),
+        BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
+          "goals", "Destinations to plan through"),
         BT::InputPort<std::string>("behavior_tree", "Behavior tree to run"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The navigate through poses error code"),

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp

@@ -34,14 +34,14 @@ class AreErrorCodesPresent : public BT::ConditionNode
     const BT::NodeConfiguration & conf)
   : BT::ConditionNode(condition_name, conf)
   {
-    getInput<std::set<unsigned short>>("error_codes_to_check", error_codes_to_check_); //NOLINT
+    getInput<std::set<uint16_t>>("error_codes_to_check", error_codes_to_check_); //NOLINT
   }
 
   AreErrorCodesPresent() = delete;
 
   BT::NodeStatus tick()
   {
-    getInput<unsigned short>("error_code", error_code_);  //NOLINT
+    getInput<uint16_t>("error_code", error_code_);  //NOLINT
 
     if (error_codes_to_check_.find(error_code_) != error_codes_to_check_.end()) {
       return BT::NodeStatus::SUCCESS;
@@ -54,14 +54,14 @@ class AreErrorCodesPresent : public BT::ConditionNode
   {
     return
       {
-        BT::InputPort<unsigned short>("error_code", "The active error codes"), //NOLINT
-        BT::InputPort<std::set<unsigned short>>("error_codes_to_check", "Error codes to check")//NOLINT
+        BT::InputPort<uint16_t>("error_code", "The active error codes"), //NOLINT
+        BT::InputPort<std::set<uint16_t>>("error_codes_to_check", "Error codes to check")//NOLINT
       };
   }
 
 protected:
-  unsigned short error_code_; //NOLINT
-  std::set<unsigned short> error_codes_to_check_; //NOLINT
+  uint16_t error_code_; //NOLINT
+  std::set<uint16_t> error_codes_to_check_; //NOLINT
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

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/condition/test_are_error_codes_present.cpp

@@ -27,8 +27,8 @@ class AreErrorCodesPresentFixture : public nav2_behavior_tree::BehaviorTreeTestF
   using ActionGoal = Action::Goal;
   void SetUp()
   {
-    int error_code = ActionGoal::NONE;
-    std::set<unsigned short> error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT
+    uint16_t error_code = ActionGoal::NONE;
+    std::set<uint16_t> error_codes_to_check = {ActionGoal::UNKNOWN}; //NOLINT
     config_->blackboard->set("error_code", error_code);
     config_->blackboard->set("error_codes_to_check", error_codes_to_check);
 
@@ -58,7 +58,7 @@ std::shared_ptr<BT::Tree> AreErrorCodesPresentFixture::tree_ = nullptr;
 
 TEST_F(AreErrorCodesPresentFixture, test_condition)
 {
-  std::map<int, BT::NodeStatus> error_to_status_map = {
+  std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
     {ActionGoal::NONE, BT::NodeStatus::FAILURE},
     {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
   };

nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp

@@ -27,7 +27,7 @@ class WouldAControllerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorT
   using ActionGoal = Action::Goal;
   void SetUp()
   {
-    int error_code = ActionGoal::NONE;
+    uint16_t error_code = ActionGoal::NONE;
     config_->blackboard->set("error_code", error_code);
 
     std::string xml_txt =
@@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldAControllerRecoveryHelpFixture::tree_ = nullptr;
 
 TEST_F(WouldAControllerRecoveryHelpFixture, test_condition)
 {
-  std::map<int, BT::NodeStatus> error_to_status_map = {
+  std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
     {ActionGoal::NONE, BT::NodeStatus::FAILURE},
     {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
     {ActionGoal::INVALID_CONTROLLER, BT::NodeStatus::FAILURE},

nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp

@@ -27,7 +27,7 @@ class WouldAPlannerRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTree
   using ActionGoal = Action::Goal;
   void SetUp()
   {
-    int error_code = ActionGoal::NONE;
+    uint16_t error_code = ActionGoal::NONE;
     config_->blackboard->set("error_code", error_code);
 
     std::string xml_txt =
@@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldAPlannerRecoveryHelpFixture::tree_ = nullptr;
 
 TEST_F(WouldAPlannerRecoveryHelpFixture, test_condition)
 {
-  std::map<int, BT::NodeStatus> error_to_status_map = {
+  std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
     {ActionGoal::NONE, BT::NodeStatus::FAILURE},
     {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
     {ActionGoal::NO_VALID_PATH, BT::NodeStatus::SUCCESS},

nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp

@@ -27,7 +27,7 @@ class WouldASmootherRecoveryHelpFixture : public nav2_behavior_tree::BehaviorTre
   using ActionGoal = Action::Goal;
   void SetUp()
   {
-    int error_code = ActionGoal::NONE;
+    uint16_t error_code = ActionGoal::NONE;
     config_->blackboard->set("error_code", error_code);
 
     std::string xml_txt =
@@ -56,7 +56,7 @@ std::shared_ptr<BT::Tree> WouldASmootherRecoveryHelpFixture::tree_ = nullptr;
 
 TEST_F(WouldASmootherRecoveryHelpFixture, test_condition)
 {
-  std::map<int, BT::NodeStatus> error_to_status_map = {
+  std::map<uint16_t, BT::NodeStatus> error_to_status_map = {
     {ActionGoal::NONE, BT::NodeStatus::FAILURE},
     {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
     {ActionGoal::TIMEOUT, BT::NodeStatus::SUCCESS},

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)

nav2_behaviors/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.2.1</version>
+  <version>1.2.2</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>