Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes

Signed-off-by: Steve Macenski <[email protected]>

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -165,7 +165,7 @@ class AckermannMotionModel : public MotionModel
     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_;
+    view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_;
   }
 
   /**

nav2_mppi_controller/test/motion_model_tests.cpp

@@ -164,6 +164,80 @@ 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 = 0; 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);
+  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);
+  }
+
+  // Check that Ackermann Drive is properly non-holonomic and parameterized
+  EXPECT_EQ(model->isHolonomic(), false);
+
+  // Check it cleanly destructs
+  model.reset();
+}
+
+TEST(MotionModelTests, AckermannReversingTest)
+{
+  models::ControlSequence control_sequence;
+  models::State state;
+  int batches = 1000;
+  int timesteps = 50;
+  control_sequence.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++) {
+    control_sequence.vx(i) = -i * i * i;  // now reversing
+    control_sequence.wz(i) = i * i * i * i;
+  }
+
+  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 = 0; 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_sequence.vx.shape(0); i++) {
+    control_sequence.vx(i) = -i * i * i;  // now reversing
+    control_sequence.wz(i) = -i * i * i * i;
+  }
+
+  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 = 0; i != control_sequence.wz.shape(0); i++) {
+    EXPECT_LT(control_sequence.wz(i), 0.0);
+  }
 
   // Now, check the specifics of the minimum curvature constraint
   EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);