Merge branch 'ros-navigation:main' into main

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v26\
+              - "<< parameters.key >>-v28\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v26\
+              - "<< parameters.key >>-v28\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v26\
+            key: "<< parameters.key >>-v28\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -29,7 +29,7 @@
 #include <vector>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "message_filters/subscriber.h"
+#include "message_filters/subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_amcl/motion_model/motion_model.hpp"
 #include "nav2_amcl/sensors/laser/laser.hpp"

nav2_amcl/src/amcl_node.cpp

@@ -28,7 +28,7 @@
 #include <utility>
 #include <vector>
 
-#include "message_filters/subscriber.h"
+#include "message_filters/subscriber.hpp"
 #include "nav2_amcl/angleutils.hpp"
 #include "nav2_util/geometry_utils.hpp"
 #include "nav2_amcl/pf/pf.hpp"

nav2_behavior_tree/CMakeLists.txt

@@ -259,6 +259,10 @@ install(DIRECTORY include/
   DESTINATION include/${PROJECT_NAME}
 )
 
+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}/${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");
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -312,18 +312,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");
 

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/remove_in_collision_goals_action.hpp

@@ -52,17 +52,23 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
+    return providedBasicPorts(
+      {
         BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
-        BT::InputPort<double>("cost_threshold", 254.0,
+        BT::InputPort<double>(
+          "cost_threshold", 254.0,
           "Cost threshold for considering a goal in collision"),
         BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
+        BT::InputPort<bool>(
+          "consider_unknown_as_obstacle", false,
+          "Whether to consider unknown cost as obstacle"),
         BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
-    });
+      });
   }
 
 private:
   bool use_footprint_;
+  bool consider_unknown_as_obstacle_;
   double cost_threshold_;
   Goals input_goals_;
 };

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
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp

@@ -24,6 +24,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "tf2_ros/buffer.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp

@@ -22,6 +22,7 @@
 
 #include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 
 namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
 #include "tf2_ros/buffer.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp

@@ -20,6 +20,7 @@
 
 #include "behaviortree_cpp/condition_node.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp

@@ -17,6 +17,7 @@
 
 #include <string>
 #include "behaviortree_cpp/behavior_tree.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp

@@ -20,6 +20,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "behaviortree_cpp/condition_node.h"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp

@@ -23,6 +23,7 @@
 #include "tf2_ros/buffer.h"
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp

@@ -23,6 +23,7 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp

@@ -25,6 +25,7 @@
 #include "nav2_util/odometry_utils.hpp"
 
 #include "behaviortree_cpp/decorator_node.h"
+#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -37,6 +37,7 @@ void RemoveInCollisionGoals::on_tick()
   getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
   getInput("input_goals", input_goals_);
+  getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 
   if (input_goals_.empty()) {
     setOutput("output_goals", input_goals_);
@@ -47,11 +48,7 @@ void RemoveInCollisionGoals::on_tick()
   request_->use_footprint = use_footprint_;
 
   for (const auto & goal : input_goals_) {
-    geometry_msgs::msg::Pose2D pose;
-    pose.x = goal.pose.position.x;
-    pose.y = goal.pose.position.y;
-    pose.theta = tf2::getYaw(goal.pose.orientation);
-    request_->poses.push_back(pose);
+    request_->poses.push_back(goal);
   }
 }
 
@@ -60,10 +57,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
 {
   Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
-    if (response->costs[i] < cost_threshold_) {
+    if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
+      response->costs[i] < cost_threshold_)
+    {
       valid_goal_poses.push_back(input_goals_[i]);
     }
   }
+  // Inform if all goals have been removed
+  if (valid_goal_poses.empty()) {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "All goals are in collision and have been removed from the list");
+  }
   setOutput("output_goals", valid_goal_poses);
   return BT::NodeStatus::SUCCESS;
 }

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -20,7 +20,6 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp

@@ -20,7 +20,6 @@
 #include "nav2_util/geometry_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp

@@ -16,7 +16,6 @@
 #include <string>
 
 #include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -20,7 +20,6 @@
 #include "nav2_util/node_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp

@@ -15,7 +15,6 @@
 #include <string>
 #include <vector>
 #include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp

@@ -13,7 +13,6 @@
 // limitations under the License.
 
 #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -26,7 +26,6 @@
 #include "behaviortree_cpp/decorator_node.h"
 
 #include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
-#include "nav2_behavior_tree/bt_utils.hpp"
 
 namespace nav2_behavior_tree
 {