Merge remote-tracking branch 'origin/main' into buildkit

.circleci/config.yml

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

.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

.github/workflows/update_ci_image.yaml

@@ -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@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           pull: true

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode
   std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
-  static std::vector<std::pair<int, int>> free_space_indices;
+  struct Point2D { int32_t x; int32_t y; };
+  static std::vector<Point2D> free_space_indices;
 #endif
 
   // Transforms
@@ -391,6 +392,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_rand_;
   std::string scan_topic_{"scan"};
   std::string map_topic_{"map"};
+  bool freespace_downsampling_ = false;
 };
 
 }  // namespace nav2_amcl

nav2_amcl/include/nav2_amcl/map/map.hpp

@@ -41,20 +41,21 @@ struct _rtk_fig_t;
 // Limits
 #define MAP_WIFI_MAX_LEVELS 8
 
-
+// make sure that the sizeof(map_cell_t) == 5
+#pragma pack(push, 1)
 // Description for a single map cell.
 typedef struct
 {
   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
-  int occ_state;
+  int8_t occ_state;
 
   // Distance to the nearest occupied cell
-  double occ_dist;
+  float occ_dist;
 
   // Wifi levels
   // int wifi_levels[MAP_WIFI_MAX_LEVELS];
 } map_cell_t;
-
+#pragma pack(pop)
 
 // Description for a map
 typedef struct

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

nav2_amcl/src/amcl_node.cpp

@@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
   add_parameter(
     "first_map_only", rclcpp::ParameterValue(false),
     "Set this to true, when you want to load a new map published from the map_server");
+
+  add_parameter(
+    "freespace_downsampling", rclcpp::ParameterValue(
+      false),
+    "Downsample the free space used by the Pose Generator. Use it with large maps to save memory");
 }
 
 AmclNode::~AmclNode()
@@ -328,7 +333,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   global_loc_srv_.reset();
   initial_guess_srv_.reset();
   nomotion_update_srv_.reset();
-  executor_thread_.reset();  //  to make sure initial_pose_sub_ completely exit
   initial_pose_sub_.reset();
   laser_scan_connection_.disconnect();
   tf_listener_.reset();  //  listener may access lase_scan_filter_, so it should be reset earlier
@@ -405,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las
 }
 
 #if NEW_UNIFORM_SAMPLING
-std::vector<std::pair<int, int>> AmclNode::free_space_indices;
+std::vector<AmclNode::Point2D> AmclNode::free_space_indices;
 #endif
 
 bool
@@ -447,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg)
 
 #if NEW_UNIFORM_SAMPLING
   unsigned int rand_index = drand48() * free_space_indices.size();
-  std::pair<int, int> free_point = free_space_indices[rand_index];
+  AmclNode::Point2D free_point = free_space_indices[rand_index];
   pf_vector_t p;
-  p.v[0] = MAP_WXGX(map, free_point.first);
-  p.v[1] = MAP_WYGY(map, free_point.second);
+  p.v[0] = MAP_WXGX(map, free_point.x);
+  p.v[1] = MAP_WYGY(map, free_point.y);
   p.v[2] = drand48() * 2 * M_PI - M_PI;
 #else
   double min_x, max_x, min_y, max_y;
@@ -536,6 +540,14 @@ 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)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");
+    return;
+  }
+
   // Overriding last published pose to initial pose
   last_published_pose_ = *msg;
 
@@ -1099,6 +1111,7 @@ AmclNode::initParameters()
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
   get_parameter("scan_topic", scan_topic_);
   get_parameter("map_topic", map_topic_);
+  get_parameter("freespace_downsampling", freespace_downsampling_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1428,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 void
 AmclNode::createFreeSpaceVector()
 {
+  int delta = freespace_downsampling_ ? 2 : 1;
   // Index of free space
   free_space_indices.resize(0);
-  for (int i = 0; i < map_->size_x; i++) {
-    for (int j = 0; j < map_->size_y; j++) {
+  for (int i = 0; i < map_->size_x; i += delta) {
+    for (int j = 0; j < map_->size_y; j += delta) {
       if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) {
-        free_space_indices.push_back(std::make_pair(i, j));
+        AmclNode::Point2D point = {i, j};
+        free_space_indices.push_back(point);
       }
     }
   }

nav2_behavior_tree/CMakeLists.txt

@@ -20,8 +20,6 @@ find_package(nav2_util REQUIRED)
 
 nav2_package()
 
-add_compile_options(-Wno-shadow) # Delete after https://github.com/BehaviorTree/BehaviorTree.CPP/issues/811 is released
-
 include_directories(
   include
 )
@@ -185,6 +183,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)
 

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -47,7 +47,8 @@ class BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
   explicit BehaviorTreeEngine(
-    const std::vector<std::string> & plugin_libraries);
+    const std::vector<std::string> & plugin_libraries,
+    rclcpp::Node::SharedPtr node);
   virtual ~BehaviorTreeEngine() {}
 
   /**
@@ -93,6 +94,9 @@ class BehaviorTreeEngine
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -100,7 +100,7 @@ class BtActionNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for %f s",
+        node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
         action_name.c_str(),
         wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -173,7 +173,7 @@ bool BtActionServer<ActionT>::on_configure()
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
-  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
+  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_);
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create();

nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp

@@ -93,8 +93,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
     RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
-        action_name.c_str());
+        node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
+        action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(
               std::string("Action server ") + action_name +
               std::string(" not available"));

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -84,8 +84,8 @@ class BtServiceNode : public BT::ActionNodeBase
       service_name_.c_str());
     if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
       RCLCPP_ERROR(
-        node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
-        service_name_.c_str());
+        node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
+        service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
       throw std::runtime_error(
               std::string(
                 "Service server %s not available",

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/package.xml

@@ -2,8 +2,8 @@
 <?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.2.0</version>
-  <description>TODO</description>
+  <version>1.3.1</version>
+  <description>Nav2 behavior tree wrappers, nodes, and utilities</description>
   <maintainer email="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">Carlos Orduno</maintainer>