diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index af7c0bb6124..4fe85bfb05a 100644 --- 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 index 8b130e311dd..6085896cfe3 100644 --- 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