Humble sync 7 August 4 1.1.9 (ros-navigation#3739)

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

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

* updating mppi's path angle critic for optional bidirectionality

* Update README.md

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

* fixing path angle critic's non-directional bias

* adding reformat

* adapting goal critic for speed to goal (ros-navigation#3641)

* adapting goal critic for speed to goal

* retuning goal critic

* add readme entries

* Update critics_tests.cpp

* Fix uninitialized value (ros-navigation#3651)

* In NAV2, this warning is treated as an error

Signed-off-by: Ryan Friedman <[email protected]>

* Fix rviz panel node arguments (ros-navigation#3655)

Signed-off-by: Nick Lamprianidis <[email protected]>

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

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

* adding nan twist rejection for velocity smoother and collision monitor

* deref

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

* alternative to path align critic for inversion control

* fix default behavior (enforce_path_inversion: false) (ros-navigation#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]>

* Fix smoother server tests (ros-navigation#3663)

* Fix smoother server tests

* Update test_smoother_server.cpp

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

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

* nav2_behaviors: export all available plugins (ros-navigation#3716)

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

* changing costmap layers private to protected (ros-navigation#3722)

* adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially (ros-navigation#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 (ros-navigation#3727)

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

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

* Fix Goal updater QoS (ros-navigation#3719)

* Fix GoalUpdater QoS

* Fixes

* bumping Humble to 1.1.9 for release

* fix merge conflict resolution in collision monitor node

---------

Signed-off-by: Ryan Friedman <[email protected]>
Signed-off-by: Nick Lamprianidis <[email protected]>
Co-authored-by: Filipe Cerveira <[email protected]>
Co-authored-by: Ryan <[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: DylanDeCoeyer-Quimesis <[email protected]>
Co-authored-by: gyaanantia <[email protected]>
Co-authored-by: Tony Najjar <[email protected]>

Author: 10 people

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.1.8</version>
+  <version>1.1.9</version>
   <description>
     <p>
       amcl is a probabilistic localization system for a robot moving in

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.1.8</version>
+  <version>1.1.9</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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/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/plugins/assisted_teleop.hpp renamed to nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp

@@ -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.1.8</version>
+  <version>1.1.9</version>
   <description>TODO</description>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>