Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect…

… sign sometimes (ros-navigation#4664)

* Fixing ros-navigation#4661: MPPI ackermann reversing taking incorrect sign sometimes

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* fixing unit test for type implicit cast

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

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_;
  }
 
  /**

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<rclcpp_lifecycle::LifecycleNode>("my_node");
+ ParametersHandler param_handler(node);
+ std::unique_ptr<AckermannMotionModel> model =
+ std::make_unique<AckermannMotionModel>(&param_handler, node->get_name());
+
+ // Check that predict properly populates the trajectory velocities with the control velocities
+ state.cvx = 10 * xt::ones<float>({batches, timesteps});
+ state.cvy = 5 * xt::ones<float>({batches, timesteps});
+ state.cwz = 1 * xt::ones<float>({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<float>({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<float>(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<float>(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();
+}