45-50% performance improvement in MPPI controller using Eigen library…

… for computation. (#4621) * Initial commit Signed-off-by: Ayush1285 <[email protected]> * Corrected to Eigen Array Signed-off-by: Ayush1285 <[email protected]> * updated motion model with eigen Signed-off-by: Ayush1285 <[email protected]> * Replaced xtensor with eigen in Optimizer, NoiseGenerator and all Util files Signed-off-by: Ayush1285 <[email protected]> * updated critics with Eigen Signed-off-by: Ayush1285 <[email protected]> * optimized Eigen::Array implementation Signed-off-by: Ayush1285 <[email protected]> * added comment Signed-off-by: Ayush1285 <[email protected]> * Updated path align critic and velocity deadband critic with Eigen Signed-off-by: Ayush1285 <[email protected]> * Updated cost critic and constraint critic with eigen Signed-off-by: Ayush1285 <[email protected]> * Updated utils test with Eigen Signed-off-by: Ayush1285 <[email protected]> * Reverted unnecessary changes and fixed static instance in Noise generator Signed-off-by: Ayush1285 <[email protected]> * changes std::abs to fabs, clamp to min-max Signed-off-by: Ayush1285 <[email protected]> * Converted tests to Eigen Signed-off-by: Ayush1285 <[email protected]> * Complete conversion from xtensor to Eigen Signed-off-by: Ayush1285 <[email protected]> * fixed few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed linters and few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed mis-merge of AckermannReversingTest Signed-off-by: Ayush1285 <[email protected]> * fixed gtest assertion Signed-off-by: Ayush1285 <[email protected]> * Fixed optimizer_unit_tests and related issues Signed-off-by: Ayush1285 <[email protected]> * Fixed all the unit tests and critic tests, all unit tests passing locally Signed-off-by: Ayush1285 <[email protected]> * fixed few review comments Signed-off-by: Ayush1285 <[email protected]> * Fixed CostCritic issue and added test for shiftColumn method Signed-off-by: Ayush1285 <[email protected]> * Added test for new functions Signed-off-by: Ayush1285 <[email protected]> * Removed compiler flags Signed-off-by: Ayush1285 <[email protected]> * updated test to check first and last columns Signed-off-by: Ayush1285 <[email protected]> * Addressed few review comments Signed-off-by: Ayush1285 <[email protected]> * Changed the obstacle critic implementation to the original way. Updated optimizer_benchmark test with critics and params Signed-off-by: Ayush1285 <[email protected]> * Fixed bugs Signed-off-by: Ayush1285 <[email protected]> * Fixed linter Signed-off-by: Ayush1285 <[email protected]> * Added clamp util function Signed-off-by: Ayush1285 <[email protected]> * Fixed bug Signed-off-by: Ayush1285 <[email protected]> * Fixed review comments: Added utils::clamp method Signed-off-by: Ayush1285 <[email protected]> * Fixing strided trajectory columns Signed-off-by: Ayush1285 <[email protected]> * fixed lint error Signed-off-by: Ayush1285 <[email protected]> * Fixed merge Signed-off-by: Ayush1285 <[email protected]> * Fixed optimizer benchmark with latest api changes Signed-off-by: Ayush1285 <[email protected]> * fixed build error Signed-off-by: Ayush1285 <[email protected]> * Fixed new util_test Signed-off-by: Ayush1285 <[email protected]> --------- Signed-off-by: Ayush1285 <[email protected]>
ros-navigation · Jan 15, 2025 · a33e8d2 · a33e8d2
1 parent 5ff8cc7
commit a33e8d2
Show file tree

Hide file tree

Showing 40 changed files with 952 additions and 872 deletions.
diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt
@@ -1,13 +1,6 @@
 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)
-
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -24,30 +17,19 @@ 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)
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIR}
+)
 
 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()
@@ -63,10 +45,9 @@ add_library(mppi_controller SHARED
   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_compile_options(mppi_controller PUBLIC -O3)
 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
@@ -84,9 +65,6 @@ target_link_libraries(mppi_controller PUBLIC
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
-  xtensor
-  xtensor::optimize
-  xtensor::use_xsimd
 )
 
 add_library(mppi_critics SHARED
@@ -102,10 +80,9 @@ add_library(mppi_critics SHARED
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
 )
-target_compile_options(mppi_critics PUBLIC -fconcepts -O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
+target_compile_options(mppi_critics PUBLIC -fconcepts -O3)
 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
@@ -122,9 +99,6 @@ target_link_libraries(mppi_critics PUBLIC
   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
@@ -150,7 +124,7 @@ if(BUILD_TESTING)
   ament_find_gtest()
 
   add_subdirectory(test)
-  # add_subdirectory(benchmark)
+  add_subdirectory(benchmark)
 endif()
 
 ament_export_libraries(${libraries})
@@ -168,7 +142,7 @@ ament_export_dependencies(
   tf2_geometry_msgs
   tf2_ros
   visualization_msgs
-  xtensor
+  Eigen3
 )
 ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_targets(nav2_mppi_controller)

diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp
@@ -13,6 +13,9 @@
 // limitations under the License.
 
 #include <benchmark/benchmark.h>
+
+#include <Eigen/Dense>
+
 #include <string>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -24,10 +27,6 @@
 #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 "nav2_mppi_controller/motion_models.hpp"
 #include "nav2_mppi_controller/controller.hpp"
 

diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp
@@ -14,6 +14,8 @@
 
 #include <benchmark/benchmark.h>
 
+#include <Eigen/Dense>
+
 #include <string>
 #include <vector>
 
@@ -27,10 +29,6 @@
 #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 "nav2_mppi_controller/optimizer.hpp"
 #include "nav2_mppi_controller/motion_models.hpp"
 
@@ -51,8 +49,8 @@ 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;
+  int batch_size = 2000;
+  int time_steps = 56;
   unsigned int path_points = 50u;
   int iteration_count = 2;
   double lookahead_distance = 10.0;
@@ -91,16 +89,17 @@ void prepareAndRunBenchmark(
   nav2_core::GoalChecker * dummy_goal_checker{nullptr};
 
   for (auto _ : state) {
-    optimizer->evalControl(pose, velocity, path, dummy_goal_checker);
+    optimizer->evalControl(pose, velocity, path, path.poses.back().pose, dummy_goal_checker);
   }
 }
 
 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 = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
+    {"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
+    {"PreferForwardCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
@@ -109,19 +108,20 @@ 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 = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
+    {"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
+    {"PreferForwardCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, 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 = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
+    {"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
+    {"PreferForwardCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }
@@ -130,8 +130,9 @@ 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 = {{"ConstraintCritic"}, {"CostCritic"}, {"GoalCritic"},
+    {"GoalAngleCritic"}, {"PathAlignCritic"}, {"PathFollowCritic"}, {"PathAngleCritic"},
+    {"PreferForwardCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp
@@ -15,16 +15,11 @@
 #ifndef NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
 #define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
 
+#include <Eigen/Dense>
+
 #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 "geometry_msgs/msg/pose_stamped.hpp"
 #include "nav2_core/goal_checker.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
@@ -48,7 +43,7 @@ struct CriticData
   const models::Path & path;
   const geometry_msgs::msg::Pose & goal;
 
-  xt::xtensor<float, 1> & costs;
+  Eigen::ArrayXf & costs;
   float & model_dt;
 
   bool fail_flag;

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp
@@ -20,13 +20,6 @@
 #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"
 

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/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.setZero(time_steps);
+    vy.setZero(time_steps);
+    wz.setZero(time_steps);
   }
 };
 

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/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.setZero(size);
+    y.setZero(size);
+    yaws.setZero(size);
   }
 };
 

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/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.setZero(batch_size, time_steps);
+    vy.setZero(batch_size, time_steps);
+    wz.setZero(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.setZero(batch_size, time_steps);
+    cvy.setZero(batch_size, time_steps);
+    cwz.setZero(batch_size, time_steps);
   }
 };
 }  // namespace mppi::models