Issue#4400 keepout speed zones

.github/mergify.yml

@@ -1,4 +1,13 @@
 pull_request_rules:
+  - name: backport to jazzy at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-jazzy"
+    actions:
+      backport:
+        branches:
+          - jazzy
+
   - name: backport to iron at reviewers discretion
     conditions:
       - base=main
@@ -17,24 +26,6 @@ pull_request_rules:
         branches:
           - humble
 
-  - name: backport to galactic at reviewers discretion
-    conditions:
-      - base=main
-      - "label=backport-galactic"
-    actions:
-      backport:
-        branches:
-          - galactic
-
-  - name: backport to foxy at reviewers discretion
-    conditions:
-      - base=main
-      - "label=backport-foxy"
-    actions:
-      backport:
-        branches:
-          - foxy-devel
-
   - name: delete head branch after merge
     conditions:
       - merged

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

nav2_amcl/src/amcl_node.cpp

@@ -312,7 +312,8 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   pose_pub_->on_deactivate();
   particle_cloud_pub_->on_deactivate();
 
-  // reset dynamic parameter handler
+  // shutdown and reset dynamic parameter handler
+  remove_on_set_parameters_callback(dyn_params_handler_.get());
   dyn_params_handler_.reset();
 
   // destroy bond connection
@@ -540,8 +541,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
       global_frame_id_.c_str());
     return;
   }
-  if (abs(msg->pose.pose.position.x) > map_->size_x ||
-    abs(msg->pose.pose.position.y) > map_->size_y)
+  if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x ||
+    abs(msg->pose.pose.position.y) > map_->size_y))
   {
     RCLCPP_ERROR(
       get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");

nav2_behavior_tree/CMakeLists.txt

@@ -185,6 +185,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node)
 add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp)
 list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node)
 
+add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp)
+list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node)
+
 add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp)
 list(APPEND plugin_libs nav2_pipeline_sequence_bt_node)
 
@@ -243,6 +246,10 @@ install(DIRECTORY test/utils/
         DESTINATION include/${PROJECT_NAME}/utils/
 )
 
+install(DIRECTORY test/utils/
+  DESTINATION include/${PROJECT_NAME}/nav2_behavior_tree/test/utils
+)
+
 install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
 install(FILES ${GENERATED_DIR}/plugins_list.hpp DESTINATION include/${PROJECT_NAME})
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -58,9 +58,7 @@ class BtActionNode : public BT::ActionNodeBase
     // Get the required items from the blackboard
     auto bt_loop_duration =
       config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
@@ -193,15 +191,19 @@ class BtActionNode : public BT::ActionNodeBase
   {
     // first step to be done only at the beginning of the Action
     if (!BT::isStatusActive(status())) {
-      // setting the status to RUNNING to notify the BT Loggers (if any)
-      setStatus(BT::NodeStatus::RUNNING);
-
       // reset the flag to send the goal or not, allowing the user the option to set it in on_tick
       should_send_goal_ = true;
 
+      // Clear the input and output messages to make sure we have no leftover from previous calls
+      goal_ = typename ActionT::Goal();
+      result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
+
       // user defined callback, may modify "should_send_goal_".
       on_tick();
 
+      // setting the status to RUNNING to notify the BT Loggers (if any)
+      setStatus(BT::NodeStatus::RUNNING);
+
       if (!should_send_goal_) {
         return BT::NodeStatus::FAILURE;
       }

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -310,18 +310,18 @@ void BtActionServer<ActionT>::executeCallback()
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:
-      RCLCPP_INFO(logger_, "Goal succeeded");
       action_server_->succeeded_current(result);
+      RCLCPP_INFO(logger_, "Goal succeeded");
       break;
 
     case nav2_behavior_tree::BtStatus::FAILED:
-      RCLCPP_ERROR(logger_, "Goal failed");
       action_server_->terminate_current(result);
+      RCLCPP_ERROR(logger_, "Goal failed");
       break;
 
     case nav2_behavior_tree::BtStatus::CANCELED:
-      RCLCPP_INFO(logger_, "Goal canceled");
       action_server_->terminate_all(result);
+      RCLCPP_INFO(logger_, "Goal canceled");
       break;
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -56,9 +56,7 @@ class BtCancelActionNode : public BT::ActionNodeBase
     callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
 
     // Get the required items from the blackboard
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -59,9 +59,7 @@ class BtServiceNode : public BT::ActionNodeBase
     // Get the required items from the blackboard
     auto bt_loop_duration =
       config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
-    server_timeout_ =
-      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
-    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+    getInputOrBlackboard("server_timeout", server_timeout_);
     wait_for_service_timeout_ =
       config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");
 
@@ -140,6 +138,9 @@ class BtServiceNode : public BT::ActionNodeBase
       // allowing the user the option to set it in on_tick
       should_send_request_ = true;
 
+      // Clear the input request to make sure we have no leftover from previous calls
+      request_ = std::make_shared<typename ServiceT::Request>();
+
       // user defined callback, may modify "should_send_request_".
       on_tick();
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

@@ -17,13 +17,15 @@
 
 #include <string>
 #include <set>
+#include <vector>
 
 #include "rclcpp/time.hpp"
 #include "rclcpp/node.hpp"
 #include "behaviortree_cpp/behavior_tree.h"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/path.hpp"
 
 namespace BT
 {
@@ -102,6 +104,70 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key)
   }
 }
 
+/**
+ * @brief Parse XML string to std::vector<geometry_msgs::msg::PoseStamped>
+ * @param key XML string
+ * @return std::vector<geometry_msgs::msg::PoseStamped>
+ */
+template<>
+inline std::vector<geometry_msgs::msg::PoseStamped> convertFromString(const StringView key)
+{
+  // 9 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() % 9 != 0) {
+    throw std::runtime_error("invalid number of fields for std::vector<PoseStamped> attribute)");
+  } else {
+    std::vector<geometry_msgs::msg::PoseStamped> poses;
+    for (size_t i = 0; i < parts.size(); i += 9) {
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
+      pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
+      pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
+      pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
+      pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
+      pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
+      pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
+      pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
+      pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
+      poses.push_back(pose_stamped);
+    }
+    return poses;
+  }
+}
+
+/**
+ * @brief Parse XML string to nav_msgs::msg::Path
+ * @param key XML string
+ * @return nav_msgs::msg::Path
+ */
+template<>
+inline nav_msgs::msg::Path convertFromString(const StringView key)
+{
+  // 9 real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if ((parts.size() - 2) % 9 != 0) {
+    throw std::runtime_error("invalid number of fields for Path attribute)");
+  } else {
+    nav_msgs::msg::Path path;
+    path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[0]));
+    path.header.frame_id = BT::convertFromString<std::string>(parts[1]);
+    for (size_t i = 2; i < parts.size(); i += 9) {
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      path.header.stamp = rclcpp::Time(BT::convertFromString<int64_t>(parts[i]));
+      pose_stamped.header.frame_id = BT::convertFromString<std::string>(parts[i + 1]);
+      pose_stamped.pose.position.x = BT::convertFromString<double>(parts[i + 2]);
+      pose_stamped.pose.position.y = BT::convertFromString<double>(parts[i + 3]);
+      pose_stamped.pose.position.z = BT::convertFromString<double>(parts[i + 4]);
+      pose_stamped.pose.orientation.x = BT::convertFromString<double>(parts[i + 5]);
+      pose_stamped.pose.orientation.y = BT::convertFromString<double>(parts[i + 6]);
+      pose_stamped.pose.orientation.z = BT::convertFromString<double>(parts[i + 7]);
+      pose_stamped.pose.orientation.w = BT::convertFromString<double>(parts[i + 8]);
+      path.poses.push_back(pose_stamped);
+    }
+    return path;
+  }
+}
+
 /**
  * @brief Parse XML string to std::chrono::milliseconds
  * @param key XML string

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -86,7 +86,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
 
 private:
   bool is_recovery_;
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -84,9 +84,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
           "error_code_id", "The back up behavior server error code")
       });
   }
-
-private:
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) 2024 Marc Morcos
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_
+
+#include <vector>
+#include <memory>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
+#include "behaviortree_cpp/action_node.h"
+#include "nav_msgs/msg/path.h"
+
+namespace nav2_behavior_tree
+{
+
+class GetPoseFromPath : public BT::ActionNodeBase
+{
+public:
+  GetPoseFromPath(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<nav_msgs::msg::Path>("path", "Path to extract pose from"),
+      BT::OutputPort<geometry_msgs::msg::PoseStamped>("pose", "Stamped Extracted Pose"),
+      BT::InputPort<int>("index", 0, "Index of pose to extract from. -1 is end of list"),
+    };
+  }
+
+private:
+  void halt() override {}
+  BT::NodeStatus tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -23,6 +23,7 @@
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "behaviortree_cpp/action_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -60,7 +61,6 @@ class RemovePassedGoals : public BT::ActionNodeBase
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string robot_base_frame_;
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp

@@ -86,7 +86,6 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 
 private:
   bool is_recovery_;
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp

@@ -61,9 +61,6 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
         BT::InputPort<double>("wait_duration", 1.0, "Wait time")
       });
   }
-
-private:
-  bool initialized_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp

@@ -34,7 +34,12 @@ class AreErrorCodesPresent : public BT::ConditionNode
     const BT::NodeConfiguration & conf)
   : BT::ConditionNode(condition_name, conf)
   {
-    getInput<std::set<uint16_t>>("error_codes_to_check", error_codes_to_check_); //NOLINT
+    std::vector<int> error_codes_to_check_vector;
+    getInput("error_codes_to_check", error_codes_to_check_vector); //NOLINT
+
+    error_codes_to_check_ = std::set<uint16_t>(
+      error_codes_to_check_vector.begin(),
+      error_codes_to_check_vector.end());
   }
 
   AreErrorCodesPresent() = delete;
@@ -55,7 +60,7 @@ class AreErrorCodesPresent : public BT::ConditionNode
     return
       {
         BT::InputPort<uint16_t>("error_code", "The active error codes"), //NOLINT
-        BT::InputPort<std::set<uint16_t>>("error_codes_to_check", "Error codes to check")//NOLINT
+        BT::InputPort<std::vector<int>>("error_codes_to_check", "Error codes to check")//NOLINT
       };
   }