Merge branch 'main' of https://github.com/ros-planning/navigation2 in…

…to main

Signed-off-by: Mannucci, Anna (Bosch (CR)) <[email protected]>

.github/workflows/update_ci_image.yaml

@@ -65,9 +65,9 @@ jobs:
     steps:
       - uses: actions/checkout@v4
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v2
+        uses: docker/setup-buildx-action@v3
       - name: Login to Docker Hub
-        uses: docker/login-action@v2
+        uses: docker/login-action@v3
         with:
           registry: ghcr.io
           username: ${{ github.repository_owner }}
@@ -99,7 +99,7 @@ jobs:
       - name: Build and push ${{ github.ref_name }}
         if: steps.config.outputs.trigger == 'true'
         id: docker_build
-        uses: docker/build-push-action@v4
+        uses: docker/build-push-action@v5
         with:
           context: .
           pull: true

nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp

@@ -49,7 +49,7 @@ typedef struct
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero();
+pf_vector_t pf_vector_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_vector_finite(pf_vector_t a);
@@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero();
+pf_matrix_t pf_matrix_zero(void);
 
 // Check for NAN or INF in any component
 // int pf_matrix_finite(pf_matrix_t a);

nav2_amcl/src/pf/pf.c

@@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   // Workspace
   double m[4], c[2][2];
-  size_t count;
   double weight;
 
   // Cluster the samples
@@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
   for (i = 0; i < set->cluster_max_count; i++) {
     cluster = set->clusters + i;
-    cluster->count = 0;
     cluster->weight = 0;
     cluster->mean = pf_vector_zero();
     cluster->cov = pf_matrix_zero();
@@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
   }
 
   // Initialize overall filter stats
-  count = 0;
   weight = 0.0;
   set->mean = pf_vector_zero();
   set->cov = pf_matrix_zero();
@@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
 
     cluster = set->clusters + cidx;
 
-    cluster->count += 1;
     cluster->weight += sample->weight;
 
-    count += 1;
     weight += sample->weight;
 
     // Compute mean

nav2_amcl/src/pf/pf_vector.c

@@ -35,7 +35,7 @@
 
 
 // Return a zero vector
-pf_vector_t pf_vector_zero()
+pf_vector_t pf_vector_zero(void)
 {
   pf_vector_t c;
 
@@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
 
 
 // Return a zero matrix
-pf_matrix_t pf_matrix_zero()
+pf_matrix_t pf_matrix_zero(void)
 {
   int i, j;
   pf_matrix_t c;

nav2_behavior_tree/README.md

@@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT
 
 See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine.
 
-For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/
+For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -162,7 +162,7 @@ class BtActionNode : public BT::ActionNodeBase
   }
 
   /**
-   * @brief Function to perform some user-defined operation whe the action is aborted.
+   * @brief Function to perform some user-defined operation when the action is aborted.
    * @return BT::NodeStatus Returns FAILURE by default, user may override return another value
    */
   virtual BT::NodeStatus on_aborted()
@@ -206,7 +206,8 @@ class BtActionNode : public BT::ActionNodeBase
       // 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>();
+        auto elapsed =
+          (node_->now() - time_goal_sent_).template 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_) {
@@ -237,7 +238,8 @@ class BtActionNode : public BT::ActionNodeBase
         {
           goal_updated_ = false;
           send_new_goal();
-          auto elapsed = (node_->now() - time_goal_sent_).to_chrono<std::chrono::milliseconds>();
+          auto elapsed =
+            (node_->now() - time_goal_sent_).template to_chrono<std::chrono::milliseconds>();
           if (!is_future_goal_handle_complete(elapsed)) {
             if (elapsed < server_timeout_) {
               return BT::NodeStatus::RUNNING;
@@ -300,6 +302,7 @@ class BtActionNode : public BT::ActionNodeBase
   void halt() override
   {
     if (should_cancel_goal()) {
+      auto future_result = action_client_->async_get_result(goal_handle_);
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
       if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
@@ -308,6 +311,16 @@ class BtActionNode : public BT::ActionNodeBase
           node_->get_logger(),
           "Failed to cancel action server for %s", action_name_.c_str());
       }
+
+      if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) !=
+        rclcpp::FutureReturnCode::SUCCESS)
+      {
+        RCLCPP_ERROR(
+          node_->get_logger(),
+          "Failed to get result for %s in node halt!", action_name_.c_str());
+      }
+
+      on_cancelled();
     }
 
     setStatus(BT::NodeStatus::IDLE);

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -196,6 +196,11 @@ class BtActionServer
    */
   void populateErrorCode(typename std::shared_ptr<typename ActionT::Result> result);
 
+  /**
+   * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs
+   */
+  void cleanErrorCodes();
+
   // Action name
   std::string action_name_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -61,6 +61,9 @@ BtActionServer<ActionT>::BtActionServer(
   if (!node->has_parameter("default_server_timeout")) {
     node->declare_parameter("default_server_timeout", 20);
   }
+  if (!node->has_parameter("action_server_result_timeout")) {
+    node->declare_parameter("action_server_result_timeout", 900.0);
+  }
 
   std::vector<std::string> error_code_names = {
     "follow_path_error_code",
@@ -123,30 +126,38 @@ bool BtActionServer<ActionT>::on_configure()
   // Support for handling the topic-based goal pose from rviz
   client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
-  // Declare parameters for client node to share with BT nodes
-  // Declare if not declared in case being used an external application
+  // Declare parameters for common client node applications to share with BT nodes
+  // Declare if not declared in case being used an external application, then copying
+  // all of the main node's parameters to the client for BT nodes to obtain
   nav2_util::declare_parameter_if_not_declared(
     node, "global_frame", rclcpp::ParameterValue(std::string("map")));
   nav2_util::declare_parameter_if_not_declared(
     node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link")));
-  client_node_->declare_parameter(
-    "robot_base_frame", node->get_parameter("robot_base_frame").as_string());
-  client_node_->declare_parameter(
-    "global_frame", node->get_parameter("global_frame").as_string());
+  nav2_util::declare_parameter_if_not_declared(
+    node, "transform_tolerance", rclcpp::ParameterValue(0.1));
+  nav2_util::copy_all_parameters(node, client_node_);
+
+  // set the timeout in seconds for the action server to discard goal handles if not finished
+  double action_server_result_timeout;
+  node->get_parameter("action_server_result_timeout", action_server_result_timeout);
+  rcl_action_server_options_t server_options = rcl_action_server_get_default_options();
+  server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout);
 
   action_server_ = std::make_shared<ActionServer>(
     node->get_node_base_interface(),
     node->get_node_clock_interface(),
     node->get_node_logging_interface(),
     node->get_node_waitables_interface(),
-    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this));
+    action_name_, std::bind(&BtActionServer<ActionT>::executeCallback, this),
+    nullptr, std::chrono::milliseconds(500), false, server_options);
 
   // Get parameters for BT timeouts
-  int timeout;
-  node->get_parameter("bt_loop_duration", timeout);
-  bt_loop_duration_ = std::chrono::milliseconds(timeout);
-  node->get_parameter("default_server_timeout", timeout);
-  default_server_timeout_ = std::chrono::milliseconds(timeout);
+  int bt_loop_duration;
+  node->get_parameter("bt_loop_duration", bt_loop_duration);
+  bt_loop_duration_ = std::chrono::milliseconds(bt_loop_duration);
+  int default_server_timeout;
+  node->get_parameter("default_server_timeout", default_server_timeout);
+  default_server_timeout_ = std::chrono::milliseconds(default_server_timeout);
 
   // Get error code id names to grab off of the blackboard
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
@@ -236,6 +247,7 @@ void BtActionServer<ActionT>::executeCallback()
 {
   if (!on_goal_received_callback_(action_server_->get_current_goal())) {
     action_server_->terminate_current();
+    cleanErrorCodes();
     return;
   }
 
@@ -290,6 +302,8 @@ void BtActionServer<ActionT>::executeCallback()
       action_server_->terminate_all(result);
       break;
   }
+
+  cleanErrorCodes();
 }
 
 template<class ActionT>
@@ -316,6 +330,14 @@ void BtActionServer<ActionT>::populateErrorCode(
   }
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::cleanErrorCodes()
+{
+  for (const auto & error_code : error_code_names_) {
+    blackboard_->set<unsigned short>(error_code, 0);  //NOLINT
+  }
+}
+
 }  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
     return basic;
   }
 
-  void halt()
+  void halt() override
   {
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase
    */
   virtual BT::NodeStatus check_future()
   {
-    auto elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+    auto elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
     auto remaining = server_timeout_ - elapsed;
 
     if (remaining > std::chrono::milliseconds(0)) {
@@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase
 
       if (rc == rclcpp::FutureReturnCode::TIMEOUT) {
         on_wait_for_result();
-        elapsed = (node_->now() - sent_time_).to_chrono<std::chrono::milliseconds>();
+        elapsed = (node_->now() - sent_time_).template to_chrono<std::chrono::milliseconds>();
         if (elapsed < server_timeout_) {
           return BT::NodeStatus::RUNNING;
         }

nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp

@@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const
     return;
   }
 
-  RCLCPP_INFO(node_->get_logger(), msg.c_str());
+  RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str());
   prev_msg = msg;
 }
 

nav2_behavior_tree/plugins/condition/transform_available_condition.cpp

@@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition(
     RCLCPP_FATAL(
       node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",
       child_frame_.c_str(), parent_frame_.c_str());
-    exit(-1);
+    throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!");
   }
 
   RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");

nav2_behavior_tree/plugins/decorator/speed_controller.cpp

@@ -43,7 +43,7 @@ SpeedController::SpeedController(
 
   if (min_rate_ <= 0.0 || max_rate_ <= 0.0) {
     std::string err_msg = "SpeedController node cannot have rate <= 0.0";
-    RCLCPP_FATAL(node_->get_logger(), err_msg.c_str());
+    RCLCPP_FATAL(node_->get_logger(), "%s", err_msg.c_str());
     throw BT::BehaviorTreeException(err_msg);
   }