Skip to content

Commit

Permalink
Adding new MPPI controller to Nav2 (ros-navigation#3350)
Browse files Browse the repository at this point in the history
* 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 (ros-navigation#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>
  • Loading branch information
4 people authored Mar 1, 2023
1 parent 8dea742 commit 8d4f6f4
Show file tree
Hide file tree
Showing 72 changed files with 9,141 additions and 4 deletions.
7 changes: 4 additions & 3 deletions .circleci/config.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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>\
Expand All @@ -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 }}\
Expand Down Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion .circleci/defaults.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ _common: &common
"build":
<<: *common
"executor": "parallel"
"parallel-workers": 2
"parallel-workers": 4
"symlink-install": true
"test":
<<: *common
Expand Down
4 changes: 4 additions & 0 deletions codecov.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
93 changes: 93 additions & 0 deletions nav2_mppi_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
22 changes: 22 additions & 0 deletions nav2_mppi_controller/LICENSE.md
Original file line number Diff line number Diff line change
@@ -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.
Loading

0 comments on commit 8d4f6f4

Please sign in to comment.