add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,129 @@
+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)
+set(XTENSOR_USE_XSIMD 1)
+
+# set(XTENSOR_DEFAULT_LAYOUT column_major)  # row_major, column_major
+# set(XTENSOR_DEFAULT_TRAVERSAL row_major)  # row_major, column_major
+
+find_package(ament_cmake REQUIRED)
+find_package(xtensor REQUIRED)
+find_package(xsimd REQUIRED)
+
+include_directories(
+  include
+)
+
+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()
+
+include(CheckCXXCompilerFlag)
+
+check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512)
+check_cxx_compiler_flag("-msse4.2" COMPILER_SUPPORTS_SSE4)
+check_cxx_compiler_flag("-mavx2" COMPILER_SUPPORTS_AVX2)
+check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA)
+
+if(COMPILER_SUPPORTS_AVX512)
+  add_compile_options(-mno-avx512f)
+endif()
+
+if(COMPILER_SUPPORTS_SSE4)
+  add_compile_options(-msse4.2)
+endif()
+
+if(COMPILER_SUPPORTS_AVX2)
+  add_compile_options(-mavx2)
+endif()
+
+if(COMPILER_SUPPORTS_FMA)
+  add_compile_options(-mfma)
+endif()
+
+# If building one the same hardware to be deployed on, try `-march=native`!
+add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
+
+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/cost_critic.cpp
+  src/critics/goal_critic.cpp
+  src/critics/goal_angle_critic.cpp
+  src/critics/path_align_critic.cpp
+  src/critics/path_align_legacy_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
+  src/critics/velocity_deadband_critic.cpp
+)
+
+set(libraries mppi_controller mppi_critics)
+
+foreach(lib IN LISTS libraries)
+  target_compile_options(${lib} PUBLIC -fconcepts)
+  target_include_directories(${lib} PUBLIC ${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()