Merge branch 'main' into eigen_mppi

.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 }}\
@@ -82,7 +82,6 @@ _commands:
         - run:
             name: Install Dependencies | << parameters.workspace >>
             working_directory: << parameters.workspace >>
-            # Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634
             command: |
               . << parameters.underlay >>/install/setup.sh
               AMENT_PREFIX_PATH=$(echo "$AMENT_PREFIX_PATH" | \
@@ -105,7 +104,6 @@ _commands:
                   --ignore-src \
                   --skip-keys " \
                     slam_toolbox \
-                    turtlebot3_gazebo \
                     " \
                   --verbose | \
                 awk '$1 ~ /^resolution\:/' | \

.github/ISSUE_TEMPLATE.md

@@ -1,7 +1,7 @@
 <!--
-For general questions, please ask on ROS answers: https://answers.ros.org, make sure to include at least the `ros2` tag and the rosdistro version you are running, e.g. `ardent`.
+For general questions, please ask on Robotics Stack Exchange: https://robotics.stackexchange.com/, make sure to include at least the `ros2` and `nav2` tags and the rosdistro version you are running, e.g. `ardent`.
 For general design discussions, please post on discourse: https://discourse.ros.org/c/ng-ros
-Not sure if this is the right repository? Open an issue on https://github.com/ros-planning/navigation2
+Not sure if this is the right repository? Open an issue on https://github.com/ros-navigation/navigation2
 For Bug report or feature requests, please fill out the relevant category below
 -->
 

.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

Dockerfile

@@ -97,13 +97,11 @@ ENV OVERLAY_WS $OVERLAY_WS
 WORKDIR $OVERLAY_WS
 COPY --from=cacher /tmp/$OVERLAY_WS ./
 
-# Remove/Replace turtlebot3_gazebo and gazebo_ros_pkgs from --skip-keys after https://github.com/ros-navigation/navigation2/pull/3634
 RUN . $UNDERLAY_WS/install/setup.sh && \
     apt-get update && rosdep install -q -y \
       --from-paths src \
       --skip-keys " \
         slam_toolbox \
-        turtlebot3_gazebo \
         "\
       --ignore-src \
     && rm -rf /var/lib/apt/lists/*
@@ -170,9 +168,7 @@ RUN mkdir -p $ROOT_SRV
 
 # install demo dependencies
 RUN apt-get update && apt-get install -y \
-      ros-$ROS_DISTRO-aws-robomaker-small-warehouse-world \
-      ros-$ROS_DISTRO-rviz2 \
-      ros-$ROS_DISTRO-turtlebot3-simulations
+      ros-$ROS_DISTRO-rviz2 
 
 # install gzweb dependacies
 RUN apt-get install -y --no-install-recommends \

nav2_amcl/CMakeLists.txt

@@ -2,90 +2,136 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_amcl)
 
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
 find_package(rclcpp_components REQUIRED)
-find_package(message_filters REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(nav2_util REQUIRED)
-find_package(nav2_msgs REQUIRED)
-find_package(pluginlib REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(
-  include
-)
-
 include(CheckSymbolExists)
 check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
 
-add_subdirectory(src/pf)
-add_subdirectory(src/map)
-add_subdirectory(src/motion_model)
-add_subdirectory(src/sensors)
+if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-gnu-folding-constant)
+endif()
 
-set(executable_name amcl)
+add_library(pf_lib SHARED
+  src/pf/pf.c
+  src/pf/pf_kdtree.c
+  src/pf/pf_pdf.c
+  src/pf/pf_vector.c
+  src/pf/eig3.c
+  src/pf/pf_draw.c
+)
+target_include_directories(pf_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+if(HAVE_DRAND48)
+  target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
+endif()
 
-add_executable(${executable_name}
-  src/main.cpp
+add_library(map_lib SHARED
+  src/map/map.c
+  src/map/map_range.c
+  src/map/map_draw.c
+  src/map/map_cspace.cpp
+)
+target_include_directories(map_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+
+add_library(motions_lib SHARED
+  src/motion_model/omni_motion_model.cpp
+  src/motion_model/differential_motion_model.cpp
+)
+target_include_directories(motions_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(motions_lib PUBLIC
+  pf_lib
+  pluginlib::pluginlib
+  nav2_util::nav2_util_core
+)
+
+add_library(sensors_lib SHARED
+  src/sensors/laser/laser.cpp
+  src/sensors/laser/beam_model.cpp
+  src/sensors/laser/likelihood_field_model.cpp
+  src/sensors/laser/likelihood_field_model_prob.cpp
+)
+target_include_directories(sensors_lib
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(sensors_lib PUBLIC
+  pf_lib
+  map_lib
 )
 
+set(executable_name amcl)
+
 set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
   src/amcl_node.cpp
 )
-
-target_include_directories(${library_name} PRIVATE src/include)
-
 if(HAVE_DRAND48)
   target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
 endif()
-
-set(dependencies
-  rclcpp
-  rclcpp_lifecycle
-  rclcpp_components
-  message_filters
-  tf2_geometry_msgs
-  geometry_msgs
-  nav_msgs
-  sensor_msgs
-  std_srvs
-  tf2_ros
-  tf2
-  nav2_util
-  nav2_msgs
-  pluginlib
+target_include_directories(${library_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${library_name} PUBLIC
+  ${geometry_msgs_TARGETS}
+  message_filters::message_filters
+  nav2_util::nav2_util_core
+  ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
+  pluginlib::pluginlib
+  sensors_lib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${nav_msgs_TARGETS}
+  tf2_ros::tf2_ros
+  tf2::tf2
+  ${nav2_msgs_TARGETS}
 )
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
+target_link_libraries(${library_name} PRIVATE
+  rclcpp_components::component
+  tf2_geometry_msgs::tf2_geometry_msgs
 )
 
-target_link_libraries(${executable_name}
-  ${library_name}
-)
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
+add_executable(${executable_name}
+  src/main.cpp
 )
-
-target_link_libraries(${library_name}
-  map_lib pf_lib sensors_lib
+target_include_directories(${executable_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${executable_name} PRIVATE
+  ${library_name}
 )
 
 rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
 
-install(TARGETS ${library_name}
+install(TARGETS ${library_name} pf_lib map_lib motions_lib sensors_lib
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -96,19 +142,34 @@ install(TARGETS ${executable_name}
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
   set(ament_cmake_copyright_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
+
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories("include/${PROJECT_NAME}")
 ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  geometry_msgs
+  message_filters
+  nav_msgs
+  nav2_msgs
+  nav2_util
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  sensor_msgs
+  std_srvs
+  tf2
+  tf2_ros
+)
+ament_export_targets(${library_name})
 pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)
 ament_package()

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -29,25 +29,21 @@
 #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"
 #include "nav2_msgs/msg/particle.hpp"
 #include "nav2_msgs/msg/particle_cloud.hpp"
 #include "nav2_msgs/srv/set_initial_pose.hpp"
 #include "nav_msgs/srv/set_map.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/node_options.hpp"
 #include "sensor_msgs/msg/laser_scan.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/message_filter.h"
 #include "tf2_ros/transform_broadcaster.h"
 #include "tf2_ros/transform_listener.h"
-#include "pluginlib/class_loader.hpp"
-
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-parameter"
-#pragma GCC diagnostic ignored "-Wreorder"
-#include "tf2_ros/message_filter.h"
-#pragma GCC diagnostic pop
 
 #define NEW_UNIFORM_SAMPLING 1
 
@@ -152,7 +148,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 +388,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