update unit test open_loop option for optimizer

Signed-off-by: Chanh Hoang <[email protected]>

Author: chanhhoang99

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -217,6 +217,44 @@ class OptimizerTester : public Optimizer
   {
     return integrateStateVelocities(traj, state);
   }
+
+  void testCloseLoop()
+  {
+    settings_.open_loop = false;
+
+    mppi::models::State s;
+    s.reset(1000,50);
+    s.speed.linear.x = 0.25;
+    s.speed.angular.z = -0.7;
+    s.speed.linear.y = 0.1;
+
+    updateInitialStateVelocities(s);
+
+    EXPECT_FLOAT_EQ(s.vx(0,0), 0.25f);
+    EXPECT_FLOAT_EQ(s.wz(0,0), -0.7f);
+    EXPECT_FLOAT_EQ(s.vy(0,0), 0.1f);
+  }
+
+  void testOpenLoop()
+  {
+    settings_.open_loop = true;
+
+    last_command_vel_.linear.x = 0.6;
+    last_command_vel_.angular.z = 0.3;
+    last_command_vel_.linear.y = 0.1;
+
+    mppi::models::State s;
+    s.reset(1000, 50);
+    s.speed.linear.x = 0.11;
+    s.speed.angular.z = -0.22;
+    s.speed.linear.y = -0.05;
+
+    updateInitialStateVelocities(s);
+
+    EXPECT_FLOAT_EQ(s.vx(0,0), 0.6f);
+    EXPECT_FLOAT_EQ(s.wz(0,0), 0.3f);
+    EXPECT_FLOAT_EQ(s.vy(0,0), 0.1f);
+  }
 };
 
 TEST(OptimizerTests, BasicInitializedFunctions)
@@ -697,6 +735,20 @@ TEST(OptimizerTests, TestGetters)
   EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f);
 }
 
+TEST(OptimizerTests, DiffDrive_ClosedLoop_usesStateSpeed)
+{
+  OptimizerTester optimizer_tester;
+  optimizer_tester.testSetOmniModel();
+  optimizer_tester.testCloseLoop();
+}
+
+TEST(OptimizerTests, DiffDrive_OpenLoop_usesLastCmd)
+{
+  OptimizerTester optimizer_tester;
+  optimizer_tester.testSetOmniModel();
+  optimizer_tester.testOpenLoop();
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);