45-50% performance improvement in MPPI controller using Eigen library for computation.

nav2_mppi_controller/CMakeLists.txt

@@ -14,9 +14,11 @@ set(XTENSOR_USE_XSIMD 1)
 find_package(ament_cmake REQUIRED)
 find_package(xsimd REQUIRED)
 find_package(xtensor REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 include_directories(
   include
+  ${EIGEN3_INCLUDE_DIR}
 )
 
 set(dependencies_pkgs
@@ -71,7 +73,7 @@ add_library(mppi_controller SHARED
   src/controller.cpp
   src/optimizer.cpp
   src/critic_manager.cpp
-  src/trajectory_visualizer.cpp
+  # src/trajectory_visualizer.cpp
   src/path_handler.cpp
   src/parameters_handler.cpp
   src/noise_generator.cpp
@@ -91,7 +93,7 @@ add_library(mppi_critics SHARED
   src/critics/velocity_deadband_critic.cpp
 )
 
-set(libraries mppi_controller mppi_critics)
+set(libraries mppi_controller mppi_critics) #
 
 foreach(lib IN LISTS libraries)
   target_compile_options(${lib} PUBLIC -fconcepts)
@@ -100,7 +102,7 @@ foreach(lib IN LISTS libraries)
   ament_target_dependencies(${lib} ${dependencies_pkgs})
 endforeach()
 
-install(TARGETS mppi_controller mppi_critics
+install(TARGETS mppi_controller
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -116,7 +118,7 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
   add_subdirectory(test)
-  # add_subdirectory(benchmark)
+  add_subdirectory(benchmark)
 endif()
 
 ament_export_libraries(${libraries})

nav2_mppi_controller/benchmark/CMakeLists.txt

@@ -13,7 +13,7 @@ foreach(name IN LISTS BENCHMARK_NAMES)
     ${dependencies_pkgs}
   )
   target_link_libraries(${name}
-    mppi_controller mppi_critics benchmark
+    mppi_controller benchmark
   )
 
 target_include_directories(${name} PRIVATE

nav2_mppi_controller/benchmark/controller_benchmark.cpp

@@ -24,9 +24,7 @@
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav2_core/goal_checker.hpp>
 
-#include <xtensor/xarray.hpp>
-#include <xtensor/xio.hpp>
-#include <xtensor/xview.hpp>
+#include <Eigen/Dense>
 
 #include "nav2_mppi_controller/motion_models.hpp"
 #include "nav2_mppi_controller/controller.hpp"
@@ -122,8 +120,7 @@ static void BM_DiffDrivePointFootprint(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "DiffDrive";
-  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
-    {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
+  std::vector<std::string> critics = {};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
@@ -132,8 +129,7 @@ static void BM_DiffDrive(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "DiffDrive";
-  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
-    {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
+  std::vector<std::string> critics = {};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
@@ -143,8 +139,7 @@ static void BM_Omni(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Omni";
-  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
-    {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
+  std::vector<std::string> critics = {};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
@@ -153,13 +148,12 @@ static void BM_Ackermann(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Ackermann";
-  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
-    {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
+  std::vector<std::string> critics = {};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
 
-static void BM_GoalCritic(benchmark::State & state)
+/*static void BM_GoalCritic(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Ackermann";
@@ -220,19 +214,19 @@ static void BM_PathAngleCritic(benchmark::State & state)
   std::vector<std::string> critics = {{"PathAngleCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
-}
+}*/
 
 BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond);
 
-BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
+/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond);
-BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);
+BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/
 
 BENCHMARK_MAIN();

nav2_mppi_controller/benchmark/optimizer_benchmark.cpp

@@ -25,9 +25,7 @@
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
 #include <nav2_core/goal_checker.hpp>
 
-#include <xtensor/xarray.hpp>
-#include <xtensor/xio.hpp>
-#include <xtensor/xview.hpp>
+#include <Eigen/Dense>
 
 #include "nav2_mppi_controller/optimizer.hpp"
 #include "nav2_mppi_controller/motion_models.hpp"
@@ -49,11 +47,11 @@ void prepareAndRunBenchmark(
   bool consider_footprint, std::string motion_model,
   std::vector<std::string> critics, benchmark::State & state)
 {
-  int batch_size = 300;
-  int time_steps = 12;
-  unsigned int path_points = 50u;
+  int batch_size = 2000;
+  int time_steps = 70;
+  unsigned int path_points = 150u;
   int iteration_count = 2;
-  double lookahead_distance = 10.0;
+  double lookahead_distance = 40.0;
 
   TestCostmapSettings costmap_settings{};
   auto costmap_ros = getDummyCostmapRos(costmap_settings);
@@ -114,15 +112,14 @@ static void BM_DiffDrive(benchmark::State & state)
 }
 
 
-static void BM_Omni(benchmark::State & state)
+/*static void BM_Omni(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Omni";
-  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
-    {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
+  std::vector<std::string> critics = {};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
-}
+}*/
 
 static void BM_Ackermann(benchmark::State & state)
 {
@@ -134,7 +131,7 @@ static void BM_Ackermann(benchmark::State & state)
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
 
-static void BM_GoalCritic(benchmark::State & state)
+/*static void BM_GoalCritic(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Ackermann";
@@ -195,19 +192,19 @@ static void BM_PathAngleCritic(benchmark::State & state)
   std::vector<std::string> critics = {{"PathAngleCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
-}
+}*/
 
 BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond);
-BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond);
+//BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond);
 
-BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
+/*BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond);
 BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond);
-BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);
+BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);*/
 
 BENCHMARK_MAIN();

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -20,7 +20,7 @@
 
 #include "nav2_mppi_controller/tools/path_handler.hpp"
 #include "nav2_mppi_controller/optimizer.hpp"
-#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
+//#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
 #include "nav2_mppi_controller/models/constraints.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 
@@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
   PathHandler path_handler_;
-  TrajectoryVisualizer trajectory_visualizer_;
+  //TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
 };

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

@@ -18,12 +18,7 @@
 #include <memory>
 #include <vector>
 
-// xtensor creates warnings that needs to be ignored as we are building with -Werror
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Warray-bounds"
-#pragma GCC diagnostic ignored "-Wstringop-overflow"
-#include <xtensor/xtensor.hpp>
-#pragma GCC diagnostic pop
+#include <Eigen/Dense>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/goal_checker.hpp"
@@ -47,7 +42,7 @@ struct CriticData
   const models::Trajectories & trajectories;
   const models::Path & path;
 
-  xt::xtensor<float, 1> & costs;
+  Eigen::ArrayXf & costs;
   float & model_dt;
 
   bool fail_flag;

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -20,12 +20,7 @@
 #include <vector>
 #include <pluginlib/class_loader.hpp>
 
-// xtensor creates warnings that needs to be ignored as we are building with -Werror
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Warray-bounds"
-#pragma GCC diagnostic ignored "-Wstringop-overflow"
-#include <xtensor/xtensor.hpp>
-#pragma GCC diagnostic pop
+
 
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"

nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp

@@ -15,12 +15,7 @@
 #ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
 #define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
 
-// xtensor creates warnings that needs to be ignored as we are building with -Werror
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Warray-bounds"
-#pragma GCC diagnostic ignored "-Wstringop-overflow"
-#include <xtensor/xtensor.hpp>
-#pragma GCC diagnostic pop
+#include <Eigen/Dense>
 
 namespace mppi::models
 {
@@ -40,15 +35,15 @@ struct Control
  */
 struct ControlSequence
 {
-  xt::xtensor<float, 1> vx;
-  xt::xtensor<float, 1> vy;
-  xt::xtensor<float, 1> wz;
+  Eigen::ArrayXf vx;
+  Eigen::ArrayXf vy;
+  Eigen::ArrayXf wz;
 
   void reset(unsigned int time_steps)
   {
-    vx = xt::zeros<float>({time_steps});
-    vy = xt::zeros<float>({time_steps});
-    wz = xt::zeros<float>({time_steps});
+    vx = Eigen::ArrayXf::Zero(time_steps);
+    vy = Eigen::ArrayXf::Zero(time_steps);
+    wz = Eigen::ArrayXf::Zero(time_steps);
   }
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp

@@ -15,12 +15,7 @@
 #ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
 #define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
 
-// xtensor creates warnings that needs to be ignored as we are building with -Werror
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Warray-bounds"
-#pragma GCC diagnostic ignored "-Wstringop-overflow"
-#include <xtensor/xtensor.hpp>
-#pragma GCC diagnostic pop
+#include <Eigen/Dense>
 
 namespace mppi::models
 {
@@ -31,18 +26,18 @@ namespace mppi::models
  */
 struct Path
 {
-  xt::xtensor<float, 1> x;
-  xt::xtensor<float, 1> y;
-  xt::xtensor<float, 1> yaws;
+  Eigen::ArrayXf x;
+  Eigen::ArrayXf y;
+  Eigen::ArrayXf yaws;
 
   /**
     * @brief Reset path data
     */
   void reset(unsigned int size)
   {
-    x = xt::zeros<float>({size});
-    y = xt::zeros<float>({size});
-    yaws = xt::zeros<float>({size});
+    x = Eigen::ArrayXf::Zero(size);
+    y = Eigen::ArrayXf::Zero(size);
+    yaws = Eigen::ArrayXf::Zero(size);
   }
 };