ros-navigation · SteveMacenski · Aug 2, 2024 · Aug 1, 2024
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
@@ -8,37 +8,25 @@ 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(angles REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
 find_package(xsimd REQUIRED)
 find_package(xtensor 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)
@@ -65,63 +53,126 @@ if(COMPILER_SUPPORTS_FMA)
 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
+  src/optimizer.cpp
+  src/parameters_handler.cpp
+  src/path_handler.cpp
+  src/trajectory_visualizer.cpp
+)
+target_compile_options(mppi_controller PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
+target_include_directories(mppi_controller
+  PUBLIC
+    ${xsimd_INCLUDE_DIRS}
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(mppi_controller PUBLIC
+  angles::angles
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::layers
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav_msgs_TARGETS}
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+  tf2_ros::tf2_ros
+  ${visualization_msgs_TARGETS}
+  xtensor
+  xtensor::optimize
+  xtensor::use_xsimd
 )
 
 add_library(mppi_critics SHARED
-  src/critics/obstacles_critic.cpp
+  src/critics/constraint_critic.cpp
   src/critics/cost_critic.cpp
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
+  src/critics/obstacles_critic.cpp
   src/critics/path_align_critic.cpp
-  src/critics/path_follow_critic.cpp
   src/critics/path_angle_critic.cpp
+  src/critics/path_follow_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()
+target_compile_options(mppi_critics PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
+target_include_directories(mppi_critics
+  PUBLIC
+    ${xsimd_INCLUDE_DIRS}
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(mppi_critics PUBLIC
+  angles::angles
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_costmap_2d::layers
+  nav2_costmap_2d::nav2_costmap_2d_core
+  ${nav_msgs_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${std_msgs_TARGETS}
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
+  tf2_ros::tf2_ros
+  ${visualization_msgs_TARGETS}
+  xtensor
+  xtensor::optimize
+  xtensor::use_xsimd
+)
+target_link_libraries(mppi_critics PRIVATE
+  pluginlib::pluginlib
+)
 
 install(TARGETS mppi_controller mppi_critics
+  EXPORT nav2_mppi_controller
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 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()
+
+  ament_find_gtest()
+
   add_subdirectory(test)
   # add_subdirectory(benchmark)
 endif()
 
 ament_export_libraries(${libraries})
-ament_export_dependencies(${dependencies_pkgs})
-ament_export_include_directories(include)
+ament_export_dependencies(
+  angles
+  geometry_msgs
+  nav2_core
+  nav2_costmap_2d
+  nav_msgs
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  visualization_msgs
+  xtensor
+)
+ament_export_include_directories(include/${PROJECT_NAME})
+ament_export_targets(nav2_mppi_controller)
+
 pluginlib_export_plugin_description_file(nav2_core mppic.xml)
 pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml)
 

diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
@@ -11,28 +11,29 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
-  <depend>rclcpp</depend>
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libomp-dev</depend>
   <depend>nav2_common</depend>
   <depend>nav2_core</depend>
-  <depend>nav2_util</depend>
   <depend>nav2_costmap_2d</depend>
-  <depend>geometry_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
-  <depend>tf2_geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>std_msgs</depend>
   <depend>tf2</depend>
-  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
-  <depend>std_msgs</depend>
-  <depend>xtensor</depend>
-  <depend>libomp-dev</depend>
-  <depend>benchmark</depend>
+  <depend>visualization_msgs</depend>
   <depend>xsimd</depend>
+  <depend>xtensor</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
     <nav2_core plugin="${prefix}/mppic.xml" />

diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt
@@ -16,25 +16,37 @@ foreach(name IN LISTS TEST_NAMES)
   ament_add_gtest(${name}
     ${name}.cpp
   )
-
-  ament_target_dependencies(${name}
-    ${dependencies_pkgs}
-  )
-
   target_link_libraries(${name}
     mppi_controller
+    ${geometry_msgs_TARGETS}
+    ${nav_msgs_TARGETS}
+    nav2_core::nav2_core
+    nav2_costmap_2d::nav2_costmap_2d_core
+    rclcpp::rclcpp
+    rclcpp_lifecycle::rclcpp_lifecycle
+    xtensor
+    xtensor::optimize
+    xtensor::use_xsimd
+    nav2_core::nav2_core
   )
 
   if(${TEST_DEBUG_INFO})
     target_compile_definitions(${name} PUBLIC -DTEST_DEBUG_INFO)
   endif()
-
 endforeach()
 
 # This is a special case requiring linking against the critics library
 ament_add_gtest(critics_tests critics_tests.cpp)
-ament_target_dependencies(critics_tests ${dependencies_pkgs})
-target_link_libraries(critics_tests mppi_controller mppi_critics)
+target_link_libraries(critics_tests
+  mppi_controller
+  mppi_critics
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  nav2_costmap_2d::nav2_costmap_2d_core
+  xtensor
+  xtensor::optimize
+  xtensor::use_xsimd
+)
 if(${TEST_DEBUG_INFO})
   target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO)
 endif()
diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp
@@ -19,6 +19,7 @@
 #include <vector>
 #include <iostream>
 #include <string_view>
+
 #include <rclcpp/executors.hpp>
 
 #include "tf2_ros/transform_broadcaster.h"