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 <[email protected]> * 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 <[email protected]> * 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 <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> * add-Wnull-dereference and fix warnings (#3622) Signed-off-by: Ryan Friedman <[email protected]> * 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 <[email protected]> * Fix rviz panel node arguments (#3655) Signed-off-by: Nick Lamprianidis <[email protected]> * 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 <[email protected]> * 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 <[email protected]> * 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 <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> * 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 <[email protected]> * 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 <[email protected]> * Update README.md (#3736) * Update README.md * Update README.md * sync iron to 1.2.2 to release --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Nick Lamprianidis <[email protected]> Signed-off-by: ymd-stella <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: Filipe Cerveira <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: RBT22 <[email protected]> Co-authored-by: Nick Lamprianidis <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: ymd-stella <[email protected]> Co-authored-by: DylanDeCoeyer-Quimesis <[email protected]> Co-authored-by: Aaditya Ravindran <[email protected]> Co-authored-by: gyaanantia <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: xianglunkai <[email protected]>
ros-navigation · Aug 4, 2023 · 3a1d41e · 3a1d41e
1 parent 123e367
commit 3a1d41e
Show file tree

Hide file tree

Showing 136 changed files with 1,409 additions and 359 deletions.
diff --git a/README.md b/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.

diff --git a/nav2_amcl/package.xml b/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

diff --git a/nav2_amcl/src/pf/eig3.c b/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;

diff --git a/nav2_behavior_tree/CMakeLists.txt b/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})

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/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());

diff --git a/...behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/...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"),

diff --git a/...r_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp b/...r_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

diff --git a/nav2_behavior_tree/package.xml b/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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>

diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/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);

diff --git a/nav2_behavior_tree/test/plugins/condition/test_are_error_codes_present.cpp b/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},
   };

diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_controller_recovery_help.cpp b/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},

diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_planner_recovery_help.cpp b/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},

diff --git a/nav2_behavior_tree/test/plugins/condition/test_would_a_smoother_recovery_help.cpp b/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},

diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/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)

diff --git a/nav2_behaviors/plugins/assisted_teleop.hpp → ...av2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/plugins/assisted_teleop.hpp → ...av2_behaviors/plugins/assisted_teleop.hpp
diff --git a/nav2_behaviors/plugins/back_up.hpp → ...nclude/nav2_behaviors/plugins/back_up.hpp b/nav2_behaviors/plugins/back_up.hpp → ...nclude/nav2_behaviors/plugins/back_up.hpp
diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp → ...v2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp → ...v2_behaviors/plugins/drive_on_heading.hpp
diff --git a/nav2_behaviors/plugins/spin.hpp → ...s/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/plugins/spin.hpp → ...s/include/nav2_behaviors/plugins/spin.hpp
diff --git a/nav2_behaviors/plugins/wait.hpp → ...s/include/nav2_behaviors/plugins/wait.hpp b/nav2_behaviors/plugins/wait.hpp → ...s/include/nav2_behaviors/plugins/wait.hpp
diff --git a/nav2_behaviors/package.xml b/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="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>