Galactic sync 5

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -238,6 +238,7 @@ class AmclNode : public nav2_util::LifecycleNode
    */
   static pf_vector_t uniformPoseGenerator(void * arg);
   pf_t * pf_{nullptr};
+  std::mutex pf_mutex_;
   bool pf_init_;
   pf_vector_t pf_odom_pose_;
   int resample_count_{0};

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

nav2_amcl/src/amcl_node.cpp

@@ -504,6 +504,8 @@ AmclNode::globalLocalizationCallback(
   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   RCLCPP_INFO(get_logger(), "Initializing with uniform distribution");
 
   pf_init_model(
@@ -528,6 +530,8 @@ AmclNode::nomotionUpdateCallback(
 void
 AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   RCLCPP_INFO(get_logger(), "initialPoseReceived");
 
   if (msg->header.frame_id == "") {
@@ -625,6 +629,8 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
 void
 AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
 {
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+
   // Since the sensor data is continually being published by the simulator or robot,
   // we don't want our callbacks to fire until we're in the active state
   if (!active_) {return;}
@@ -930,7 +936,7 @@ AmclNode::publishAmclPose(
   if (!initial_pose_is_known_) {
     if (checkElapsedTime(2s, last_time_printed_msg_)) {
       RCLCPP_WARN(
-        get_logger(), "ACML cannot publish a pose or update the transform. "
+        get_logger(), "AMCL cannot publish a pose or update the transform. "
         "Please set the initial pose...");
       last_time_printed_msg_ = now();
     }

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -183,41 +183,17 @@ class BtActionNode : public BT::ActionNodeBase
       send_new_goal();
     }
 
-    // if new goal was sent and action server has not yet responded
-    // check the future goal handle
-    if (future_goal_handle_) {
-      auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
-      if (!is_future_goal_handle_complete(elapsed)) {
-        // return RUNNING if there is still some time before timeout happens
-        if (elapsed < server_timeout_) {
-          return BT::NodeStatus::RUNNING;
-        }
-        // if server has taken more time to respond than the specified timeout value return FAILURE
-        RCLCPP_WARN(
-          node_->get_logger(),
-          "Timed out while waiting for action server to acknowledge goal request for %s",
-          action_name_.c_str());
-        future_goal_handle_.reset();
-        return BT::NodeStatus::FAILURE;
-      }
-    }
-
-    // The following code corresponds to the "RUNNING" loop
-    if (rclcpp::ok() && !goal_result_available_) {
-      // user defined callback. May modify the value of "goal_updated_"
-      on_wait_for_result();
-
-      auto goal_status = goal_handle_->get_status();
-      if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
-        goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
-      {
-        goal_updated_ = false;
-        send_new_goal();
+    try {
+      // if new goal was sent and action server has not yet responded
+      // check the future goal handle
+      if (future_goal_handle_) {
         auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
         if (!is_future_goal_handle_complete(elapsed)) {
+          // return RUNNING if there is still some time before timeout happens
           if (elapsed < server_timeout_) {
             return BT::NodeStatus::RUNNING;
           }
+          // if server has taken more time than the specified timeout value return FAILURE
           RCLCPP_WARN(
             node_->get_logger(),
             "Timed out while waiting for action server to acknowledge goal request for %s",
@@ -227,12 +203,48 @@ class BtActionNode : public BT::ActionNodeBase
         }
       }
 
-      callback_group_executor_.spin_some();
+      // The following code corresponds to the "RUNNING" loop
+      if (rclcpp::ok() && !goal_result_available_) {
+        // user defined callback. May modify the value of "goal_updated_"
+        on_wait_for_result();
+
+        auto goal_status = goal_handle_->get_status();
+        if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+          goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
+        {
+          goal_updated_ = false;
+          send_new_goal();
+          auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+          if (!is_future_goal_handle_complete(elapsed)) {
+            if (elapsed < server_timeout_) {
+              return BT::NodeStatus::RUNNING;
+            }
+            RCLCPP_WARN(
+              node_->get_logger(),
+              "Timed out while waiting for action server to acknowledge goal request for %s",
+              action_name_.c_str());
+            future_goal_handle_.reset();
+            return BT::NodeStatus::FAILURE;
+          }
+        }
 
-      // check if, after invoking spin_some(), we finally received the result
-      if (!goal_result_available_) {
-        // Yield this Action, returning RUNNING
-        return BT::NodeStatus::RUNNING;
+        callback_group_executor_.spin_some();
+
+        // check if, after invoking spin_some(), we finally received the result
+        if (!goal_result_available_) {
+          // Yield this Action, returning RUNNING
+          return BT::NodeStatus::RUNNING;
+        }
+      }
+    } catch (const std::runtime_error & e) {
+      if (e.what() == std::string("send_goal failed") ||
+        e.what() == std::string("Goal was rejected by the action server"))
+      {
+        // Action related failure that should not fail the tree, but the node
+        return BT::NodeStatus::FAILURE;
+      } else {
+        // Internal exception to propogate to the tree
+        throw e;
       }
     }
 

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.0.7</version>
+  <version>1.0.8</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

nav2_bringup/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.0.7</version>
+  <version>1.0.8</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_bringup/nav2_gazebo_spawner/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_gazebo_spawner</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>Package for spawning a robot model into Gazebo for Nav2</description>
   <maintainer email="carlos.orduno@intel.com">lkumarbe</maintainer>
   <maintainer email="lalit.kumar.begani@intel.com">lkumarbe</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.0.7</version>
+  <version>1.0.8</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -47,13 +47,12 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  if (!node->has_parameter("default_nav_through_poses_bt_xml")) {
-    std::string pkg_share_dir =
-      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-    std::string tree_file = pkg_share_dir +
-      "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml";
-    node->declare_parameter("default_nav_through_poses_bt_xml", tree_file);
-  }
+  std::string pkg_share_dir =
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  node->declare_parameter<std::string>(
+    "default_nav_through_poses_bt_xml",
+    pkg_share_dir +
+    "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
   node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -53,13 +53,12 @@ NavigateToPoseNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
-    std::string pkg_share_dir =
-      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-    std::string tree_file = pkg_share_dir +
-      "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml";
-    node->declare_parameter("default_nav_to_pose_bt_xml", tree_file);
-  }
+  std::string pkg_share_dir =
+    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+  node->declare_parameter<std::string>(
+    "default_nav_to_pose_bt_xml",
+    pkg_share_dir +
+    "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
   node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;

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.0.7</version>
+  <version>1.0.8</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_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.0.7</version>
+  <version>1.0.8</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.0.7</version>
+  <version>1.0.8</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/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_costmap_2d</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>
     This package provides an implementation of a 2D costmap that takes in sensor
     data from the world, builds a 2D or 3D occupancy grid of the data (depending

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -100,7 +100,7 @@ void KeepoutFilter::filterInfoCallback(
   }
 
   // Checking that base and multiplier are set to their default values
-  if (msg->base != BASE_DEFAULT or msg->multiplier != MULTIPLIER_DEFAULT) {
+  if (msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT) {
     RCLCPP_ERROR(
       logger_,
       "KeepoutFilter: For proper use of keepout filter base and multiplier"

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -333,13 +333,11 @@ void VoxelLayer::raytraceFreespace(
     publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0);
   }
 
-  if (publish_clearing_points) {
-    clearing_endpoints_->data.clear();
-    clearing_endpoints_->width = clearing_observation.cloud_->width;
-    clearing_endpoints_->height = clearing_observation.cloud_->height;
-    clearing_endpoints_->is_dense = true;
-    clearing_endpoints_->is_bigendian = false;
-  }
+  clearing_endpoints_->data.clear();
+  clearing_endpoints_->width = clearing_observation.cloud_->width;
+  clearing_endpoints_->height = clearing_observation.cloud_->height;
+  clearing_endpoints_->is_dense = true;
+  clearing_endpoints_->is_bigendian = false;
 
   sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_);
   modifier.setPointCloud2Fields(

nav2_dwb_controller/costmap_queue/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>The costmap_queue package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_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>dwb_core</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>TODO</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_critics/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_msgs/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>dwb_msgs</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/dwb_plugins/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>
       Standard implementations of the GoalChecker
       and TrajectoryGenerators for dwb_core

nav2_dwb_controller/nav2_dwb_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_dwb_controller</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

nav2_dwb_controller/nav_2d_msgs/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>nav_2d_msgs</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_dwb_controller/nav_2d_utils/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>nav_2d_utils</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

nav2_lifecycle_manager/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_lifecycle_manager</name>
-  <version>1.0.7</version>
+  <version>1.0.8</version>
   <description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>