implemented code change from ros-navigation#3529 and added a test for it

Signed-off-by: Aarish Shah <shahaarish00@gmail.com>

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -93,6 +93,16 @@ class MotionModel
       float wz_last = state.wz(i, 0);
       for (unsigned int j = 1; j != state.vx.shape(1); j++) {
         float & cvx_curr = state.cvx(i, j - 1);
+        // Accelerating if magnitude of v_cmd is above magnitude of v_curr
+        // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing through 0.0)
+        // Decelerating otherwise
+        if (abs(cvx_curr) >= abs(vx_last) && cvx_curr * vx_last >= 0.0) {
+          max_delta_vx = model_dt_ * control_constraints_.ax_max;
+          min_delta_vx = -model_dt_ * control_constraints_.ax_max;
+        } else {
+          max_delta_vx = -model_dt_ * control_constraints_.ax_min;
+          min_delta_vx = model_dt_ * control_constraints_.ax_min;
+        }
         cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
         state.vx(i, j) = cvx_curr;
         vx_last = cvx_curr;

nav2_mppi_controller/test/motion_model_tests.cpp

@@ -20,7 +20,8 @@
 #include "nav2_mppi_controller/motion_models.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/models/control_sequence.hpp"
-
+#include <iostream>
+#include <xtensor/xio.hpp>
 // Tests motion models
 
 class RosLockGuard
@@ -125,6 +126,31 @@ TEST(MotionModelTests, OmniTest)
   model.reset();
 }
 
+TEST(MotionModelTests, OmniBidirectionalAccelDecelTest)
+{
+  // models::ControlSequence control_sequence;
+  models::State state;
+  int batches = 1;
+  int timesteps = 5;
+  // control_sequence.reset(timesteps);  // populates with zeros
+  state.reset(batches, timesteps);  // populates with zeros
+  std::unique_ptr<OmniMotionModel> model =
+    std::make_unique<OmniMotionModel>();
+  models::ControlConstraints control_constraints{10.0f, -10.0f, 5.0f, 1.0f, 2.0f, -1.0f, 1.0f,
+  1.0f};
+  float model_dt = 1;
+  model->initialize(control_constraints, model_dt);
+
+  state.cvx = xt::xtensor<float,2>({{-1,-3,-6,-6,-6}});
+  // state.cvx = -1 * xt::xtensor<float,2>({{10,10,9,8,6,4,3,3,3,3}});
+  xt::view(state.vx, xt::all(), 0) = 0;
+
+  model->predict(state);
+  xt::xtensor<float,2> expected_state_vx({{0,-1,-3,-5,-6}});
+  EXPECT_EQ(state.vx, expected_state_vx);
+  std::cout << state.vx << std::endl;
+}
+
 TEST(MotionModelTests, AckermannTest)
 {
   models::ControlSequence control_sequence;