Galactic sync 5 (#2748)

* Fix typo (#2583)

fix typo in warning msg;

* Stopping networking-related BT failures from failing task, rather fail specific BT node (#2624)

* addig try/catch around non-tree exceptions to fail node

* Update bt_action_node.hpp

* linting

* refactor of #2624 for simplicity (#2629)

* removing kinematic limiting from RPP (#2631)

* removing kinematic limiting from RPP

* update tests

* Bug fix: set the BT Navigator "default_nav_to_pose_bt_xml" and "default_nav_through_pose_bt_xml" from the yaml file (#2664)

* removing the has_parameter() and replacing with the get_parameter()

* fixing navigate_through_poses as well

* Adding comment to clarify intent

* Footprint collision checking for nav2_regulated_pure_pursuit (#2556)

* initial

* initial

* initialize

* un

* working

* lint fix

* wrk

* clean_up

* remove code repettion

* rm unused variable

* initialize footprint on configure itself

* revert to defaulted constructer

* remove rviz change

* remove unneccessary changes

* review changes

* define traverse unkown

* set true

* reviw changes

* styling changes

* collision checking condition

* used back()

* make loop end condition fixed

* review changes

* fix

* removed midpose curvature

* removed stop collision check at goal pose

* Windows fix (#2672)

* Corrected reinitialization in nav2_smac_planner::HybridMotionTable (#2680)

- The HybridMotionTable did not reinitialize properly in case the min_turning_radius or the motion_model was changed.

* Lifecycle startup fix (#2694)

* removed init_timer from lifecycle callback group

* lifecycle manager: startup() on autostart_ calling from callback_group_
(Bond::waitUntilFormed needs node spinning in the background)

Co-authored-by: Matej Vargovcik <[email protected]>

* If not downsampling costmap, default factor to 1 (#2695)

* Theta star planner fix (#2703)

* add test to check if the goal is unsafe with theta star planner

Signed-off-by: Daisuke Sato <[email protected]>

* check if a plan is generated (#2698)

Signed-off-by: Daisuke Sato <[email protected]>

* Always initialize clearing endpoints in voxel layer (#2705)

* Prohibit of simultaneous usage of callbacks accessing to pf_ object (#2712)

* Fixed min lookahead dist when moving backwards (#2726)

Co-authored-by: seasony-vp <[email protected]>

* bumping galactic to 1.0.8 for sync

Co-authored-by: lantica <[email protected]>
Co-authored-by: Pradheep Krishna <[email protected]>
Co-authored-by: Sathakkadhullah <[email protected]>
Co-authored-by: Tobias Fischer <[email protected]>
Co-authored-by: zoltan-lengyel <[email protected]>
Co-authored-by: RoboTech Vision <[email protected]>
Co-authored-by: Matej Vargovcik <[email protected]>
Co-authored-by: Daisuke Sato <[email protected]>
Co-authored-by: M. Mostafa Farzan <[email protected]>
Co-authored-by: Alexey Merzlyakov <[email protected]>
Co-authored-by: dpm-seasony <[email protected]>
Co-authored-by: seasony-vp <[email protected]>

Author: 13 people

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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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="[email protected]">lkumarbe</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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;