Implement Critic for Velocity Deadband Hardware Constraints (ros-navi…

…gation#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov <[email protected]> * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov <[email protected]> * remove old weight Signed-off-by: Denis Sokolov <[email protected]> * fix velocity deadband critic tests Signed-off-by: Denis Sokolov <[email protected]> --------- Signed-off-by: Denis Sokolov <[email protected]>
mini-1235 · Dec 12, 2024 · 7908967 · 7908967
1 parent 1c2c5f8
commit 7908967
Showing 1 changed file with 49 additions and 0 deletions.
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
@@ -535,3 +535,52 @@ TEST(CriticTests, PathAlignCritic)
   critic.score(data);
   EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
 }
+
+TEST(CriticTests, VelocityDeadbandCritic)
+{
+  // Standard preamble
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", "dummy_costmap", true);
+  ParametersHandler param_handler(node);
+  auto getParam = param_handler.getParamGetter("critic");
+  std::vector<double> deadband_velocities_;
+  getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{0.08, 0.08, 0.08});
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  models::State state;
+  models::ControlSequence control_sequence;
+  models::Trajectories generated_trajectories;
+  models::Path path;
+  xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
+  float model_dt = 0.1;
+  CriticData data =
+  {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt,
+    std::nullopt};
+  data.motion_model = std::make_shared<OmniMotionModel>();
+
+  // Initialization testing
+
+  // Make sure initializes correctly and that defaults are reasonable
+  VelocityDeadbandCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+
+  // Scoring testing
+
+  // provide velocities out of deadband bounds, should not have any costs
+  state.vx = 0.80 * xt::ones<float>({1000, 30});
+  state.vy = 0.60 * xt::ones<float>({1000, 30});
+  state.wz = 0.80 * xt::ones<float>({1000, 30});
+  critic.score(data);
+  EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
+
+  // Test cost value
+  state.vx = 0.01 * xt::ones<float>({1000, 30});
+  state.vy = 0.02 * xt::ones<float>({1000, 30});
+  state.wz = 0.021 * xt::ones<float>({1000, 30});
+  critic.score(data);
+  // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
+  EXPECT_NEAR(costs(1), 19.845, 0.01);
+}