Update upstream

.circleci/config.yml

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

.github/mergify.yml

@@ -1,4 +1,13 @@
 pull_request_rules:
+  - name: backport to humble at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-humble"
+    actions:
+      backport:
+        branches:
+          - humble
+
   - name: backport to galactic at reviewers discretion
     conditions:
       - base=main

.github/workflows/update_ci_image.yaml

@@ -63,9 +63,9 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v1
+        uses: docker/setup-buildx-action@v2
       - name: Login to Docker Hub
-        uses: docker/login-action@v1
+        uses: docker/login-action@v2
         with:
           registry: ghcr.io
           username: ${{ github.repository_owner }}
@@ -94,7 +94,7 @@ jobs:
       - name: Build and push
         if: steps.config.outputs.trigger == 'true'
         id: docker_build
-        uses: docker/build-push-action@v2
+        uses: docker/build-push-action@v3
         with:
           pull: true
           push: true

.gitignore

@@ -44,9 +44,13 @@ install
 # Python artifacts
 __pycache__/
 *.py[cod]
+.ipynb_checkpoints
 
 sphinx_doc/_build
 
 # CLion artifacts
 .idea
 cmake-build-debug/
+
+# doxygen docs
+doc/html/

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -91,10 +91,26 @@ class AmclNode : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
   // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
   // respond until we're in the active state
   std::atomic<bool> active_{false};
 
+  // Dedicated callback group and executor for services and subscriptions in AmclNode,
+  // in order to isolate TF timer used in message filter.
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
   // Pose hypothesis
   typedef struct
   {
@@ -132,7 +148,7 @@ class AmclNode : public nav2_util::LifecycleNode
   bool first_map_only_{true};
   std::atomic<bool> first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;
-  std::recursive_mutex configuration_mutex_;
+  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;
@@ -155,7 +171,8 @@ class AmclNode : public nav2_util::LifecycleNode
    * @brief Initialize incoming data message subscribers and filters
    */
   void initMessageFilters();
-  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_;
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
   message_filters::Connection laser_scan_connection_;
 
@@ -238,7 +255,6 @@ class AmclNode : public nav2_util::LifecycleNode
    */
   static pf_vector_t uniformPoseGenerator(void * arg);
   pf_t * pf_{nullptr};
-  std::mutex pf_mutex_;
   bool pf_init_;
   pf_vector_t pf_odom_pose_;
   int resample_count_{0};

nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp

@@ -19,8 +19,8 @@
  *
  */
 
-#ifndef DIFFERENTIAL_MOTION_MODEL_HPP
-#define DIFFERENTIAL_MOTION_MODEL_HPP
+#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
 
 #include <sys/types.h>
 #include <math.h>
@@ -44,4 +44,4 @@ class DifferentialMotionModel : public nav2_amcl::MotionModel
   double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
 };
 }  // namespace nav2_amcl
-#endif
+#endif  // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_

nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

@@ -18,9 +18,10 @@
 #define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
 
 #include <string>
+#include <memory>
+
 #include "nav2_amcl/pf/pf.hpp"
 #include "nav2_amcl/pf/pf_pdf.hpp"
-#include <memory>
 
 namespace nav2_amcl
 {
@@ -55,7 +56,6 @@ class MotionModel
    * @param delta change in pose in odometry update
    */
   virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
-
 };
 }  // namespace nav2_amcl
 

nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp

@@ -19,8 +19,8 @@
  *
  */
 
-#ifndef DIFFERENTIAL_MOTION_MODEL_HPP
-#define DIFFERENTIAL_MOTION_MODEL_HPP
+#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
 
 #include <sys/types.h>
 #include <math.h>
@@ -44,4 +44,4 @@ class OmniMotionModel : public nav2_amcl::MotionModel
   double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
 };
 }  // namespace nav2_amcl
-#endif
+#endif  // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_

nav2_amcl/include/nav2_amcl/pf/eig3.hpp

@@ -1,3 +1,23 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
+ *                      gerkey@usc.edu    kaspers@robotics.usc.edu
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
 /* Eigen-decomposition for symmetric 3x3 real matrices.
    Public domain, copied from the public domain Java library JAMA. */
 

nav2_amcl/include/nav2_amcl/portable_utils.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) Samsung Research America
+//
+// 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_AMCL__PORTABLE_UTILS_HPP_
+#define NAV2_AMCL__PORTABLE_UTILS_HPP_
+
+#include <stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef HAVE_DRAND48
+// Some system (e.g., Windows) doesn't come with drand48(), srand48().
+// Use rand, and srand for such system.
+static double drand48(void)
+{
+  return ((double)rand()) / RAND_MAX;// NOLINT
+}
+
+static void srand48(long int seedval)// NOLINT
+{
+  srand(seedval);
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__PORTABLE_UTILS_HPP_

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