Fixed review comments: Added utils::clamp method

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

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -30,6 +30,13 @@
 namespace mppi
 {
 
+// Forward declaration of utils method, since utils.hpp can't be included here due
+// to recursive inclusion.
+namespace utils
+{
+float clamp(const float lower_bound, const float upper_bound, const float input);
+}
+
 /**
  * @class mppi::MotionModel
  * @brief Abstract motion model for modeling a vehicle
@@ -79,18 +86,18 @@ class MotionModel
       for (unsigned int j = 0; j != n_rows; j++) {
         float vx_last = state.vx(j, i - 1);
         float & cvx_curr = state.cvx(j, i - 1);
-        cvx_curr = std::min(vx_last + max_delta_vx, std::max(cvx_curr, vx_last + min_delta_vx));
+        cvx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, cvx_curr);
         state.vx(j, i) = cvx_curr;
 
         float wz_last = state.wz(j, i - 1);
         float & cwz_curr = state.cwz(j, i - 1);
-        cwz_curr = std::min(wz_last + max_delta_wz, std::max(cwz_curr, wz_last - max_delta_wz));
+        cwz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, cwz_curr);
         state.wz(j, i) = cwz_curr;
 
         if (is_holo) {
           float vy_last = state.vy(j, i - 1);
           float & cvy_curr = state.cvy(j, i - 1);
-          cvy_curr = std::min(vy_last + max_delta_vy, std::max(cvy_curr, vy_last - max_delta_vy));
+          cvy_curr = utils::clamp(vy_last - max_delta_vy, vy_last + max_delta_vy, cvy_curr);
           state.vy(j, i) = cvy_curr;
         }
       }
@@ -152,11 +159,8 @@ class AckermannMotionModel : public MotionModel
     for(int i = 0; i < steps; i++) {
       float wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_);
       float & wz_curr = *(wz_ptr + i);
-      wz_curr = std::min(wz_constrained, std::max(wz_curr, -1 * wz_constrained));
+      wz_curr = utils::clamp(-1 * wz_constrained, wz_constrained, wz_curr);
     }
-    // Taking more time compared to for loop
-    // wz = ((vx.abs() / wz.abs() < min_turning_r_).select(
-    // wz, wz.sign() * vx / min_turning_r_)).eval();
   }
 
   /**

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -690,7 +690,7 @@ inline auto normalize_yaws_between_points(
  * @param upper_bound Upper bound.
  * @return Clamped output.
  */
-inline auto clamp(
+inline float clamp(
   const float lower_bound, const float upper_bound, const float input)
 {
   return std::min(upper_bound, std::max(input, lower_bound));

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -187,10 +187,10 @@ void CostCritic::score(CriticData & data)
   }
 
   if (power_ > 1u) {
-    data.costs += (std::move(repulsive_cost) *
+    data.costs += (repulsive_cost *
       (weight_ / static_cast<float>(strided_traj_cols))).pow(power_);
   } else {
-    data.costs += std::move(repulsive_cost) * (weight_ / static_cast<float>(strided_traj_cols));
+    data.costs += repulsive_cost * (weight_ / static_cast<float>(strided_traj_cols));
   }
 
   data.fail_flag = all_trajectories_collide;

nav2_mppi_controller/src/optimizer.cpp

@@ -320,10 +320,12 @@ void Optimizer::integrateStateVelocities(
     last_yaw = curr_yaw;
   }
 
-  utils::shiftColumnsByOnePlace(traj_yaws, 1);
-  traj_yaws(0) = initial_yaw;
   Eigen::ArrayXf yaw_cos = traj_yaws.cos();
   Eigen::ArrayXf yaw_sin = traj_yaws.sin();
+  utils::shiftColumnsByOnePlace(yaw_cos, 1);
+  utils::shiftColumnsByOnePlace(yaw_sin, 1);
+  yaw_cos(0) = cosf(initial_yaw);
+  yaw_sin(0) = sinf(initial_yaw);
 
   auto dx = (vx * yaw_cos).eval();
   auto dy = (vx * yaw_sin).eval();
@@ -359,10 +361,13 @@ void Optimizer::integrateStateVelocities(
   for(unsigned int i = 1; i != n_cols; i++) {
     trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt;
   }
-  utils::shiftColumnsByOnePlace(trajectories.yaws, 1);
-  trajectories.yaws.col(0) = initial_yaw;
-  auto yaw_cos = trajectories.yaws.cos().eval();
-  auto yaw_sin = trajectories.yaws.sin().eval();
+
+  Eigen::ArrayXXf yaw_cos = trajectories.yaws.cos();
+  Eigen::ArrayXXf yaw_sin = trajectories.yaws.sin();
+  utils::shiftColumnsByOnePlace(yaw_cos, 1);
+  utils::shiftColumnsByOnePlace(yaw_sin, 1);
+  yaw_cos.col(0) = cosf(initial_yaw);
+  yaw_sin.col(0) = sinf(initial_yaw);
 
   auto dx = (state.vx * yaw_cos).eval();
   auto dy = (state.vx * yaw_sin).eval();

nav2_mppi_controller/test/motion_model_tests.cpp

@@ -20,6 +20,7 @@
 #include "nav2_mppi_controller/motion_models.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
 
 // Tests motion models