Complete conversion from xtensor to Eigen

Signed-off-by: Ayush1285 <[email protected]>

nav2_mppi_controller/CMakeLists.txt

@@ -1,19 +1,7 @@
 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(
@@ -97,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()
 

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -18,12 +18,7 @@
 #include <memory>
 #include <string>
 
-// 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 "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -79,7 +74,7 @@ class TrajectoryVisualizer
     * @brief Add an optimal trajectory to visualize
     * @param trajectory Optimal trajectory
     */
-  void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
+  void add(const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace);
 
   /**
     * @brief Add candidate trajectories to visualize

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -708,30 +708,34 @@ auto rollColumns(T&& e, std::ptrdiff_t shift)
 
 inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws, const Eigen::ArrayXf & yaws_between_points)
 {
-  const auto yaws_ptr = yaws.data();
-  const auto yaws_between_points_ptr = yaws_between_points.data();
   int size = yaws.size();
   Eigen::ArrayXf yaws_between_points_corrected(size);
-  auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data();
   for(int i = 0; i != size; i++)
   {
-    const float & yaw_between_points = *(yaws_between_points_ptr + i);
-    *(yaws_between_points_corrected_ptr + i) = *(yaws_ptr + i) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
+    const float & yaw_between_points = yaws_between_points[i];
+    yaws_between_points_corrected[i] = yaws[i] < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
   }
+
+  // binaryExpr slower than for loop
+  // Eigen::ArrayXf yaws_between_points_corrected = yaws.binaryExpr(yaws_between_points, [&](const float & yaw, const float & yaw_between_points)
+  //     {return yaw < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);});
   return yaws_between_points_corrected;
 }
 
 inline auto point_corrected_yaws(const Eigen::ArrayXf & yaws_between_points, const float & goal_yaw)
 {
-  const auto yaws_between_points_ptr = yaws_between_points.data();
   int size = yaws_between_points.size();
   Eigen::ArrayXf yaws_between_points_corrected(size);
-  auto yaws_between_points_corrected_ptr = yaws_between_points_corrected.data();
   for(int i = 0; i != size; i++)
   {
-    const float & yaw_between_points = *(yaws_between_points_ptr + i);
-    *(yaws_between_points_corrected_ptr + i) = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
+    const float & yaw_between_points = yaws_between_points[i];
+    yaws_between_points_corrected[i] = fabs(angles::normalize_angle(yaw_between_points - goal_yaw)) < M_PIF_2 ? 
+        yaw_between_points : angles::normalize_angle(yaw_between_points + M_PIF);
   }
+
+  // unaryExpr slower than for loop
+  // Eigen::ArrayXf yaws_between_points_corrected = yaws_between_points.unaryExpr([&](const float & yaw_between_points)
+  //     {return fabs(normalize_anglef(yaw_between_points - goal_yaw)) < M_PIF_2 ? yaw_between_points : normalize_anglef(yaw_between_points + M_PIF);});
   return yaws_between_points_corrected;
 }
 

nav2_mppi_controller/package.xml

@@ -25,10 +25,8 @@
   <depend>tf2_eigen</depend>
   <depend>tf2_ros</depend>
   <depend>std_msgs</depend>
-  <depend>xtensor</depend>
   <depend>libomp-dev</depend>
   <depend>benchmark</depend>
-  <depend>xsimd</depend>
   <depend>eigen3_cmake_module</depend>
   <depend>eigen</depend>
 

nav2_mppi_controller/src/critics/path_angle_critic.cpp

@@ -148,4 +148,4 @@ void PathAngleCritic::score(CriticData & data)
 
 PLUGINLIB_EXPORT_CLASS(
   mppi::critics::PathAngleCritic,
-  mppi::critics::CriticFunction)
+  mppi::critics::CriticFunction)

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -59,7 +59,7 @@ void TrajectoryVisualizer::on_deactivate()
 void TrajectoryVisualizer::add(
   const Eigen::ArrayXXf & trajectory, const std::string & marker_namespace)
 {
-  auto & size = trajectory.rows();
+  size_t size = trajectory.rows();
   if (!size) {
     return;
   }
@@ -78,21 +78,21 @@ void TrajectoryVisualizer::add(
       points_->markers.push_back(marker);
     };
 
-  for (size_t i = 0; i < size; i++) {
+  for (size_t i = 0; i != size; i++) {
     add_marker(i);
   }
 }
 
 void TrajectoryVisualizer::add(
   const models::Trajectories & trajectories, const std::string & marker_namespace)
 {
-  int n_rows = trajectories.x.rows();
-  int n_cols = trajectories.x.cols();
+  size_t n_rows = trajectories.x.rows();
+  size_t n_cols = trajectories.x.cols();
   const float shape_1 = static_cast<float>(n_cols);
   points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_));
 
-  for (size_t i = 0; i < n_rows; i += trajectory_step_) {
-    for (size_t j = 0; j < n_cols; j += time_step_) {
+  for (size_t i = 0; i != n_rows; i += trajectory_step_) {
+    for (size_t j = 0; j != n_cols; j += time_step_) {
       const float j_flt = static_cast<float>(j);
       float blue_component = 1.0f - j_flt / shape_1;
       float green_component = j_flt / shape_1;
@@ -128,4 +128,4 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
   }
 }
 
-}  // namespace mppi
+}  // namespace mppi

nav2_mppi_controller/test/CMakeLists.txt

@@ -12,6 +12,7 @@ set(TEST_NAMES
   optimizer_unit_tests
 )
 
+
 foreach(name IN LISTS TEST_NAMES)
   ament_add_gtest(${name}
     ${name}.cpp
@@ -31,10 +32,10 @@ foreach(name IN LISTS TEST_NAMES)
 
 endforeach()
 
-This is a special case requiring linking against the critics library
+# 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)
 if(${TEST_DEBUG_INFO})
   target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO)
-endif()
+endif()