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

nav2_mppi_controller/CMakeLists.txt

@@ -1,22 +1,12 @@
 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(xsimd REQUIRED)
-find_package(xtensor REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 include_directories(
   include
+  ${EIGEN3_INCLUDE_DIR}
 )
 
 set(dependencies_pkgs
@@ -95,8 +85,8 @@ 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)
+  target_include_directories(${lib} PUBLIC)  # ${OpenMP_INCLUDE_DIRS}
+  target_link_libraries(${lib})
   ament_target_dependencies(${lib} ${dependencies_pkgs})
 endforeach()
 
@@ -116,7 +106,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 mppi_critics
   )
 
 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"

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"
@@ -118,8 +116,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);
 }

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);
   }
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp

@@ -15,16 +15,12 @@
 #ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
 #define NAV2_MPPI_CONTROLLER__MODELS__STATE_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>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+
 namespace mppi::models
 {
 
@@ -34,13 +30,13 @@ namespace mppi::models
  */
 struct State
 {
-  xt::xtensor<float, 2> vx;
-  xt::xtensor<float, 2> vy;
-  xt::xtensor<float, 2> wz;
+  Eigen::ArrayXXf vx;
+  Eigen::ArrayXXf vy;
+  Eigen::ArrayXXf wz;
 
-  xt::xtensor<float, 2> cvx;
-  xt::xtensor<float, 2> cvy;
-  xt::xtensor<float, 2> cwz;
+  Eigen::ArrayXXf cvx;
+  Eigen::ArrayXXf cvy;
+  Eigen::ArrayXXf cwz;
 
   geometry_msgs::msg::PoseStamped pose;
   geometry_msgs::msg::Twist speed;
@@ -50,13 +46,13 @@ struct State
     */
   void reset(unsigned int batch_size, unsigned int time_steps)
   {
-    vx = xt::zeros<float>({batch_size, time_steps});
-    vy = xt::zeros<float>({batch_size, time_steps});
-    wz = xt::zeros<float>({batch_size, time_steps});
+    vx = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    vy = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    wz = Eigen::ArrayXXf::Zero(batch_size, time_steps);
 
-    cvx = xt::zeros<float>({batch_size, time_steps});
-    cvy = xt::zeros<float>({batch_size, time_steps});
-    cwz = xt::zeros<float>({batch_size, time_steps});
+    cvx = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    cvy = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    cwz = Eigen::ArrayXXf::Zero(batch_size, time_steps);
   }
 };
 }  // namespace mppi::models

nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp

@@ -15,13 +15,7 @@
 #ifndef NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_
 #define NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_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>
-#include <xtensor/xview.hpp>
-#pragma GCC diagnostic pop
+#include <Eigen/Dense>
 
 namespace mppi::models
 {
@@ -32,18 +26,18 @@ namespace mppi::models
  */
 struct Trajectories
 {
-  xt::xtensor<float, 2> x;
-  xt::xtensor<float, 2> y;
-  xt::xtensor<float, 2> yaws;
+  Eigen::ArrayXXf x;
+  Eigen::ArrayXXf y;
+  Eigen::ArrayXXf yaws;
 
   /**
     * @brief Reset state data
     */
   void reset(unsigned int batch_size, unsigned int time_steps)
   {
-    x = xt::zeros<float>({batch_size, time_steps});
-    y = xt::zeros<float>({batch_size, time_steps});
-    yaws = xt::zeros<float>({batch_size, time_steps});
+    x = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    y = Eigen::ArrayXXf::Zero(batch_size, time_steps);
+    yaws = Eigen::ArrayXXf::Zero(batch_size, time_steps);
   }
 };