Merge remote-tracking branch 'upstream/main' into DynamicParamPatterns

Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com>

Author: Nils-ChristianIseke

.github/PULL_REQUEST_TEMPLATE.md

@@ -13,6 +13,13 @@
 
 ---
 
+## Description of testing performed
+<!--
+  For example: Linting validation using -> pre-commit run --all,
+    Package testing using -> colcon test --packages-select <modified package>,
+    or functional testing of changes on the robot or in simulation
+-->
+
 ## Description of contribution in a few bullet points
 
 <!--

.github/workflows/lint.yml

@@ -11,33 +11,34 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-          linter: [xmllint, cpplint, uncrustify, pep257, flake8]
+          linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
     - uses: actions/checkout@v4
+
+    - name: Install typeshed for mypy
+      if: matrix.linter == 'mypy'
+      run: sudo apt update && sudo apt install -y python3-typeshed
+
     - uses: ros-tooling/action-ros-lint@v0.1
       with:
         linter: ${{ matrix.linter }}
         distribution: rolling
         package-name: "*"
+        arguments: ${{ matrix.linter == 'mypy' && '--config tools/pyproject.toml' || '' }}
 
-  ament_lint_mypy:
-    name: ament_mypy
+  pre-commit:
+    name: pre-commit
     runs-on: ubuntu-latest
-    container:
-      image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
-    strategy:
-      matrix:
-        mypy_packages:
-          - "nav2_smac_planner"
     steps:
-    - uses: actions/checkout@v4
-
-    - name: Install typeshed
-      run: sudo apt update && sudo apt install -y python3-typeshed
-
-    - uses: ros-tooling/action-ros-lint@v0.1
-      with:
-        linter: mypy
-        distribution: rolling
-        package-name: "${{ join(matrix.mypy_packages, ' ') }}"
-        arguments: --config tools/pyproject.toml
+      - uses: actions/checkout@v4
+      - uses: actions/setup-python@v5
+      - uses: pre-commit/action@v3.0.1
+        env:
+          SKIP: >-
+            ament_lint_cmake,
+            ament_cpplint,
+            ament_uncrustify,
+            ament_xmllint,
+            ament_flake8,
+            ament_pep257,
+            ament_mypy

.pre-commit-config.yaml

@@ -95,3 +95,10 @@ repos:
         language: system
         types: [python]
         entry: ament_pep257
+    -   id: ament_mypy
+        name: ament_mypy
+        description: Check Python code style using mypy.
+        language: system
+        types: [python]
+        args: ["--config", "tools/pyproject.toml"]
+        entry: ament_mypy

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -47,6 +47,7 @@
 #include "pluginlib/class_loader.hpp"
 #include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
+#include "nav2_util/service_server.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -206,7 +207,8 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize state services
    */
   void initServices();
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr global_loc_srv_;
   /*
    * @brief Service callback for a global relocalization request
    */
@@ -216,7 +218,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   // service server for providing an initial pose guess
-  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr initial_guess_srv_;
   /*
    * @brief Service callback for an initial pose guess request
    */
@@ -226,7 +229,8 @@ class AmclNode : public nav2_util::LifecycleNode
     std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
 
   // Let amcl update samples without requiring motion
-  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
+  nav2_util::ServiceServer<std_srvs::srv::Empty,
+    std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr nomotion_update_srv_;
   /*
    * @brief Request an AMCL update even though the robot hasn't moved
    */

nav2_amcl/src/amcl_node.cpp

@@ -1564,18 +1564,21 @@ AmclNode::initPubSub()
 void
 AmclNode::initServices()
 {
-  global_loc_srv_ = create_service<std_srvs::srv::Empty>(
-    "reinitialize_global_localization",
+  global_loc_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "reinitialize_global_localization", shared_from_this(),
     std::bind(&AmclNode::globalLocalizationCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
-  initial_guess_srv_ = create_service<nav2_msgs::srv::SetInitialPose>(
-    "set_initial_pose",
+  initial_guess_srv_ = std::make_shared<nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "set_initial_pose", shared_from_this(),
     std::bind(&AmclNode::initialPoseReceivedSrv, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
-  nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
-    "request_nomotion_update",
+  nomotion_update_srv_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Empty,
+      std::shared_ptr<nav2_util::LifecycleNode>>>(
+    "request_nomotion_update", shared_from_this(),
     std::bind(&AmclNode::nomotionUpdateCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 }

nav2_amcl/src/map/map.c

@@ -49,6 +49,9 @@ map_t * map_alloc(void)
   // Allocate storage for main map
   map->cells = (map_cell_t *) NULL;
 
+  // Initialize max_occ_dist to 0.0
+  map->max_occ_dist = 0.0;
+
   return map;
 }
 

nav2_amcl/src/map/map_cspace.cpp

@@ -25,15 +25,15 @@
 #include "nav2_amcl/map/map.hpp"
 
 /*
- * @class CellData
+ * @struct CellData
  * @brief Data about map cells
  */
-class CellData
+struct CellData
 {
-public:
   map_t * map_;
   unsigned int i_, j_;
   unsigned int src_i_, src_j_;
+  float occ_dist;
 };
 
 /*
@@ -82,9 +82,7 @@ class CachedDistanceMap
  */
 bool operator<(const CellData & a, const CellData & b)
 {
-  return a.map_->cells[MAP_INDEX(
-             a.map_, a.i_,
-             a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
+  return a.occ_dist > b.occ_dist;
 }
 
 /*
@@ -118,7 +116,9 @@ void enqueue(
   CachedDistanceMap * cdm,
   unsigned char * marked)
 {
-  if (marked[MAP_INDEX(map, i, j)]) {
+  const int map_index = MAP_INDEX(map, i, j);
+
+  if (marked[map_index]) {
     return;
   }
 
@@ -130,18 +130,13 @@ void enqueue(
     return;
   }
 
-  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
-
-  CellData cell;
-  cell.map_ = map;
-  cell.i_ = i;
-  cell.j_ = j;
-  cell.src_i_ = src_i;
-  cell.src_j_ = src_j;
+  map->cells[map_index].occ_dist = distance * map->scale;
 
-  Q.push(cell);
+  Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
+      static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
+      map->cells[map_index].occ_dist});
 
-  marked[MAP_INDEX(map, i, j)] = 1;
+  marked[map_index] = 1;
 }
 
 /*
@@ -164,16 +159,18 @@ void map_update_cspace(map_t * map, double max_occ_dist)
   // Enqueue all the obstacle cells
   CellData cell;
   cell.map_ = map;
+  int loop_map_index;
   for (int i = 0; i < map->size_x; i++) {
     cell.src_i_ = cell.i_ = i;
     for (int j = 0; j < map->size_y; j++) {
-      if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
+      loop_map_index = MAP_INDEX(map, i, j);
+      if (map->cells[loop_map_index].occ_state == +1) {
+        map->cells[loop_map_index].occ_dist = 0.0;
         cell.src_j_ = cell.j_ = j;
-        marked[MAP_INDEX(map, i, j)] = 1;
+        marked[loop_map_index] = 1;
         Q.push(cell);
       } else {
-        map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
+        map->cells[loop_map_index].occ_dist = max_occ_dist;
       }
     }
   }
@@ -182,27 +179,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
     CellData current_cell = Q.top();
     if (current_cell.i_ > 0) {
       enqueue(
-        map, current_cell.i_ - 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ - 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (current_cell.j_ > 0) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ - 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ - 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
       enqueue(
-        map, current_cell.i_ + 1, current_cell.j_,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_ + 1, current_cell.j_,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
     if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
       enqueue(
-        map, current_cell.i_, current_cell.j_ + 1,
-        current_cell.src_i_, current_cell.src_j_,
-        Q, cdm, marked);
+          map, current_cell.i_, current_cell.j_ + 1,
+          current_cell.src_i_, current_cell.src_j_,
+          Q, cdm, marked);
     }
 
     Q.pop();

nav2_amcl/src/sensors/laser/likelihood_field_model.cpp

@@ -35,7 +35,15 @@ LikelihoodFieldModel::LikelihoodFieldModel(
   z_hit_ = z_hit;
   z_rand_ = z_rand;
   sigma_hit_ = sigma_hit;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 double

nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp

@@ -43,7 +43,15 @@ LikelihoodFieldModelProb::LikelihoodFieldModelProb(
   beam_skip_distance_ = beam_skip_distance;
   beam_skip_threshold_ = beam_skip_threshold;
   beam_skip_error_threshold_ = beam_skip_error_threshold;
-  map_update_cspace(map, max_occ_dist);
+
+  // recompute cspace only when necessary, i.e. if:
+  // - max_occ_dist changed
+  // OR
+  // - cspace was not computed yet, i.e. when map->max_occ_dist == 0.0 (and hence different from
+  // max_occ_dist)
+  if (map->max_occ_dist != max_occ_dist) {
+    map_update_cspace(map, max_occ_dist);
+  }
 }
 
 // Determine the probability for the given pose

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -22,6 +22,7 @@
 
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
+#include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "behaviortree_cpp/xml_parsing.h"
 
 #include "rclcpp/rclcpp.hpp"
@@ -85,6 +86,18 @@ class BehaviorTreeEngine
     const std::string & file_path,
     BT::Blackboard::Ptr blackboard);
 
+  /**
+   * @brief Add Groot2 monitor to publish BT status changes
+   * @param tree BT to monitor
+   * @param server_port Groot2 Server port, first of the pair (server_port, publisher_port)
+   */
+  void addGrootMonitoring(BT::Tree * tree, uint16_t server_port);
+
+  /**
+   * @brief Reset groot monitor
+   */
+  void resetGrootMonitor();
+
   /**
    * @brief Function to explicitly reset all BT nodes to initial state
    * @param tree Tree to halt
@@ -97,6 +110,9 @@ class BehaviorTreeEngine
 
   // Clock
   rclcpp::Clock::SharedPtr clock_;
+
+  // Groot2 monitor
+  std::unique_ptr<BT::Groot2Publisher> groot_monitor_;
 };
 
 }  // namespace nav2_behavior_tree