Humble backport of MPPI controller (#3439)

* Adding new MPPI controller to Nav2 (#3350)

* adding new MPPI controller to Nav2

* fixing rename for Nav2 staging

* using larger resource class

* fix plugin name

* wz typo

* add mppi gif

* Update defaults.yaml

* Update makeflags
to match core count of resource_class: large

* Bump cache version
for testing CI changes

* fixing tests

* remove unused function

* Update config.yml

* adding a little more detail

* adding contextual note

* adding contextual exceptions

* Fix using different frame for global and local costmap (#3425)

* getGlobalPlanConsideringBoundsInCostmapFrame

Replace transformPlanPosesToCostmapFrame and getGlobalPlanConsideringBounds by getGlobalPlanConsideringBoundsInCostmapFrame

* use stamp from robot pose for transform

* style

* fix test

* lint test

---------

Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

---------

Co-authored-by: ruffsl <roxfoxpox@gmail.com>
Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
(cherry picked from commit 8d4f6f4)

* Replace nav2_core exceptions with std::runtime_error

* Get inflation layer parameters from node params

* remove changes unrelated to mppi

* fix eol

* Add reset behavior (draft)

* initialize last_time_called_

* add readme for reset_period

* Revert "remove changes unrelated to mppi"

This reverts commit 55fec35.

* changing MPPI's SG filter to 9-point formulation (prev. 5) (#3444)

* changing filter to 9

* fix tests

(cherry picked from commit 7aee1e7)

* Adapt tests to humble Costmap2DROS constructor

* cpplint

* Update nav2_mppi_controller/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* address time comment

* fix API change rclcpp::ServicesQoS()

* fix CI

---------

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v12\
+              - "<< parameters.key >>-v13\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v12\
+              - "<< parameters.key >>-v13\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v12\
+            key: "<< parameters.key >>-v13\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\
@@ -453,6 +453,7 @@ executors:
   release_exec:
     docker:
       - image: ghcr.io/ros-planning/navigation2:main
+    resource_class: large
     working_directory: /opt/overlay_ws
     environment:
       <<: *common_environment

.circleci/defaults.yaml

@@ -6,7 +6,7 @@ _common: &common
 "build":
   <<: *common
   "executor": "parallel"
-  "parallel-workers": 2
+  "parallel-workers": 4
   "symlink-install": true
 "test":
   <<: *common

codecov.yml

@@ -7,3 +7,7 @@ ignore:
   - "*/test/**/*" # ignore package  test directories, e.g. nav2_costmap_2d/tests
   - "**/test_*.*" # ignore files starting with test_ e.g.  nav2_map_server/test/test_constants.cpp
   - "**/*_tests.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp
+  - "*/**/benchmark/*"  # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests
+  - "*/benchmark/**/*" # ignore package  test directories, e.g. nav2_costmap_2d/tests
+  - "**/benchmark_*.*" # ignore files starting with test_ e.g.  nav2_map_server/test/test_constants.cpp
+  - "**/*_benchmark.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -64,7 +64,7 @@ class BtServiceNode : public BT::ActionNodeBase
     getInput("service_name", service_name_);
     service_client_ = node_->create_client<ServiceT>(
       service_name_,
-      rmw_qos_profile_services_default,
+      rclcpp::ServicesQoS(),
       callback_group_);
 
     // Make a request for the service without parameter

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -62,13 +62,13 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   manager_srv_ = create_service<ManageLifecycleNodes>(
     get_name() + std::string("/manage_nodes"),
     std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),
-    rmw_qos_profile_services_default,
+    rclcpp::ServicesQoS(),
     callback_group_);
 
   is_active_srv_ = create_service<std_srvs::srv::Trigger>(
     get_name() + std::string("/is_active"),
     std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),
-    rmw_qos_profile_services_default,
+    rclcpp::ServicesQoS(),
     callback_group_);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;

nav2_mppi_controller/CMakeLists.txt

@@ -0,0 +1,93 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_mppi_controller)
+
+add_definitions(-DXTENSOR_ENABLE_XSIMD)
+add_definitions(-DXTENSOR_USE_XSIMD)
+
+set(XTENSOR_USE_TBB 0)
+set(XTENSOR_USE_OPENMP 0)
+
+
+find_package(ament_cmake REQUIRED)
+find_package(xtensor REQUIRED)
+
+set(dependencies_pkgs
+  rclcpp
+  nav2_common
+  pluginlib
+  tf2
+  geometry_msgs
+  visualization_msgs
+  nav_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav2_util
+  tf2_geometry_msgs
+  tf2_eigen
+  tf2_ros
+)
+
+foreach(pkg IN LISTS dependencies_pkgs)
+  find_package(${pkg} REQUIRED)
+endforeach()
+
+nav2_package()
+add_compile_options(-O3 -mavx2 -mfma -finline-limit=1000000 -ffp-contract=fast -ffast-math)
+
+add_library(mppi_controller SHARED
+  src/controller.cpp
+  src/optimizer.cpp
+  src/critic_manager.cpp
+  src/trajectory_visualizer.cpp
+  src/path_handler.cpp
+  src/parameters_handler.cpp
+  src/noise_generator.cpp
+)
+
+add_library(mppi_critics SHARED
+  src/critics/obstacles_critic.cpp
+  src/critics/goal_critic.cpp
+  src/critics/goal_angle_critic.cpp
+  src/critics/path_align_critic.cpp
+  src/critics/path_follow_critic.cpp
+  src/critics/path_angle_critic.cpp
+  src/critics/prefer_forward_critic.cpp
+  src/critics/twirling_critic.cpp
+  src/critics/constraint_critic.cpp
+)
+
+set(libraries mppi_controller mppi_critics)
+
+foreach(lib IN LISTS libraries)
+  target_compile_options(${lib} PUBLIC -fconcepts)
+  target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS})
+  target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd)
+  ament_target_dependencies(${lib} ${dependencies_pkgs})
+endforeach()
+
+install(TARGETS mppi_controller mppi_critics
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+  # add_subdirectory(benchmark)
+endif()
+
+ament_export_libraries(${libraries})
+ament_export_dependencies(${dependencies_pkgs})
+ament_export_include_directories(include)
+pluginlib_export_plugin_description_file(nav2_core mppic.xml)
+pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml)
+
+ament_package()

nav2_mppi_controller/LICENSE.md

@@ -0,0 +1,22 @@
+MIT License
+
+Copyright (c) 2021-2022 Fast Sense Studio
+Copyright (c) 2022-2023 Samsung Research America
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.