Merge pull request #39 from logivations/fix-bug-mppi

Fix bug mppi
logivations · Jul 11, 2023 · f4f22cd · f4f22cd
2 parents 4a82bf2 + 796cccb
commit f4f22cd
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -112,7 +112,7 @@ class AckermannMotionModel : public MotionModel
     auto & vx = control_sequence.vx;
     auto & wz = control_sequence.wz;
 
-    auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) > min_turning_r_);
+    auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_);
     view = xt::sign(wz) * vx / min_turning_r_;
   }
 

diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp
@@ -156,7 +156,7 @@ TEST(MotionModelTests, AckermannTest)
   // Check that application of constraints are non-empty for Ackermann Drive
   for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
     control_sequence.vx(i) = i * i * i;
-    control_sequence.wz(i) = i * i * i;
+    control_sequence.wz(i) = i * i * i * i;
   }
 
   models::ControlSequence initial_control_sequence = control_sequence;
@@ -168,7 +168,7 @@ TEST(MotionModelTests, AckermannTest)
   // Now, check the specifics of the minimum curvature constraint
   EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
   for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) {
-    EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) <= 0.2);
+    EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) >= 0.2);
   }
 
   // Check that Ackermann Drive is properly non-holonomic and parameterized