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 9e303f1a21..6b466773ac 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -164,8 +164,8 @@ 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_); - view = xt::sign(wz) * vx / min_turning_r_; + auto view = xt::masked_view(wz, (xt::fabs(vx) / xt::fabs(wz)) < min_turning_r_); + view = xt::sign(wz) * xt::fabs(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 4238fa03fb..f22424d226 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -164,6 +164,9 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -177,3 +180,78 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } + +TEST(MotionModelTests, AckermannReversingTest) +{ + models::ControlSequence control_sequence; + models::ControlSequence control_sequence2; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + control_sequence2.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler, node->get_name()); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence.vx(i) = -idx * idx * idx; // now reversing + control_sequence.wz(i) = idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } + + // Repeat with negative rotation direction + for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence2.vx(i) = -idx * idx * idx; // now reversing + control_sequence2.wz(i) = -idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence2 = control_sequence2; + model->applyConstraints(control_sequence2); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); + EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) { + EXPECT_LT(control_sequence2.wz(i), 0.0); + } + + // Now, check the specifics of the minimum curvature constraint + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) { + EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +}