Humble sync 7 August 4 1.1.9

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="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/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.1.8</version>
+  <version>1.1.9</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -14,7 +14,7 @@
 
 #include <utility>
 
-#include "assisted_teleop.hpp"
+#include "nav2_behaviors/plugins/assisted_teleop.hpp"
 #include "nav2_util/node_utils.hpp"
 
 namespace nav2_behaviors

nav2_behaviors/plugins/back_up.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "back_up.hpp"
+#include "nav2_behaviors/plugins/back_up.hpp"
 
 namespace nav2_behaviors
 {

nav2_behaviors/plugins/drive_on_heading.cpp

@@ -14,7 +14,7 @@
 // limitations under the License.
 
 #include <memory>
-#include "drive_on_heading.hpp"
+#include "nav2_behaviors/plugins/drive_on_heading.hpp"
 
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior)

nav2_behaviors/plugins/spin.cpp

@@ -18,7 +18,7 @@
 #include <memory>
 #include <utility>
 
-#include "spin.hpp"
+#include "nav2_behaviors/plugins/spin.hpp"
 #include "tf2/utils.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_util/node_utils.hpp"

nav2_behaviors/plugins/wait.cpp

@@ -15,7 +15,7 @@
 #include <chrono>
 #include <memory>
 
-#include "wait.hpp"
+#include "nav2_behaviors/plugins/wait.hpp"
 
 namespace nav2_behaviors
 {

nav2_bringup/launch/rviz_launch.py

@@ -71,7 +71,8 @@ def generate_launch_description():
         namespace=namespace,
         arguments=['-d', namespaced_rviz_config_file],
         output='screen',
-        remappings=[('/tf', 'tf'),
+        remappings=[('/map', 'map'),
+                    ('/tf', 'tf'),
                     ('/tf_static', 'tf_static'),
                     ('/goal_pose', 'goal_pose'),
                     ('/clicked_point', 'clicked_point'),

nav2_bringup/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_bringup</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_bt_navigator/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_bt_navigator</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -204,8 +204,15 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
 void
 NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
 {
+  geometry_msgs::msg::PoseStamped current_pose;
+  nav2_util::getCurrentPose(
+    current_pose, *feedback_utils_.tf,
+    feedback_utils_.global_frame, feedback_utils_.robot_frame,
+    feedback_utils_.transform_tolerance);
+
   RCLCPP_INFO(
-    logger_, "Begin navigating from current location to (%.2f, %.2f)",
+    logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)",
+    current_pose.pose.position.x, current_pose.pose.position.y,
     goal->pose.pose.position.x, goal->pose.pose.position.y);
 
   // Reset state for new action feedback

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -27,6 +27,7 @@
 #include "tf2_ros/transform_listener.h"
 
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
 #include "nav2_collision_monitor/polygon.hpp"

nav2_collision_monitor/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_collision_monitor</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -148,6 +148,12 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 
 void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg)
 {
+  // If message contains NaN or Inf, ignore
+  if (!nav2_util::validateTwist(*msg)) {
+    RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
+    return;
+  }
+
   process({msg->linear.x, msg->linear.y, msg->angular.z});
 }
 

nav2_collision_monitor/src/range.cpp

@@ -78,7 +78,7 @@ void Range::getData(
 
   // Ignore data, if its range is out of scope of range sensor abilities
   if (data_->range < data_->min_range || data_->range > data_->max_range) {
-    RCLCPP_WARN(
+    RCLCPP_DEBUG(
       logger_,
       "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...",
       source_name_.c_str(), data_->range, data_->min_range, data_->max_range);

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -128,17 +128,21 @@ def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
                 yaml[key] = rewrite_val
                 break
             key = yaml_key_list.pop(0)
-            yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val)
-
+            if isinstance(yaml, list):
+                yaml[int(key)] = self.updateYamlPathVals(yaml[int(key)], yaml_key_list, rewrite_val)
+            else:
+                yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val)
         return yaml
 
     def substitute_keys(self, yaml, key_rewrites):
         if len(key_rewrites) != 0:
-            for key, val in yaml.items():
-                if isinstance(val, dict) and key in key_rewrites:
+            for key in list(yaml.keys()):
+                val = yaml[key]
+                if key in key_rewrites:
                     new_key = key_rewrites[key]
                     yaml[new_key] = yaml[key]
                     del yaml[key]
+                if isinstance(val, dict):
                     self.substitute_keys(val, key_rewrites)
 
     def getYamlLeafKeys(self, yamlData):

nav2_common/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_common</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_constrained_smoother/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_constrained_smoother</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

nav2_controller/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_controller</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

nav2_core/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_core</name>
-  <version>1.1.8</version>
+  <version>1.1.9</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -178,7 +178,6 @@ class InflationLayer : public Layer
    */
   void onFootprintChanged() override;
 
-private:
   /**
    * @brief  Lookup pre-computed distances
    * @param mx The x coordinate of the current cell

nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp

@@ -128,7 +128,7 @@ class RangeSensorLayer : public CostmapLayer
    */
   void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message);
 
-private:
+protected:
   /**
    * @brief Processes all sensors into the costmap buffered from callbacks
    */

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -123,7 +123,7 @@ class StaticLayer : public CostmapLayer
    */
   virtual void matchSize();
 
-private:
+protected:
   /**
    * @brief Get parameters of layer
    */

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -133,7 +133,6 @@ class VoxelLayer : public ObstacleLayer
    */
   virtual void resetMaps();
 
-private:
   /**
    * @brief Use raycasting between 2 points to clear freespace
    */