- add cost test

- fix linting issue Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com> Add README Signed-off-by: Denis Sokolov <denis.sokolov48@gmail.com>
ros-navigation · Apr 11, 2024 · 817228c · 817228c
1 parent 9bb290f
commit 817228c
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 4 deletions.
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -171,6 +171,15 @@ Uses inflated costmap cost directly to avoid obstacles
  | cost_weight | double | Default 10.0. Weight to apply to critic term. |
  | cost_power | int | Default 1. Power order to apply to term. |
 
+
+#### Velocity Deadband Critic
+ | Parameter | Type | Definition |
+ | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
+ | cost_weight | double | Default 100.0. Weight to apply to critic term. (100.0 fits for turtlebot3, 35.0 for linorobot2) |
+ | cost_power | int | Default 1. Power order to apply to term. |
+ | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. |
+
+
 ### XML configuration example
 ```
 controller_server:
@@ -200,7 +209,7 @@ controller_server:
  time_step: 3
  AckermannConstraints:
  min_turning_r: 0.2
- critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+ critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", "VelocityDeadbandCritic"]
  ConstraintCritic:
  enabled: true
  cost_power: 1
@@ -262,6 +271,11 @@ controller_server:
  threshold_to_consider: 0.5
  max_angle_to_furthest: 1.0
  forward_preference: true
+ VelocityDeadbandCritic:
+ enabled: false
+ cost_power: 1
+ cost_weight: 100.0
+ deadband_velocities: [0.08, 0.08, 0.08]
  # TwirlingCritic:
  # enabled: true
  # twirling_cost_power: 1

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp
@@ -15,6 +15,8 @@
 #ifndef NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_
 #define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_
 
+#include <vector>
+
 #include "nav2_mppi_controller/critic_function.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"

diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp
@@ -22,7 +22,7 @@ void VelocityDeadbandCritic::initialize()
  auto getParam = parameters_handler_->getParamGetter(name_);
 
  getParam(power_, "cost_power", 1);
- getParam(weight_, "cost_weight", 4.0);
+ getParam(weight_, "cost_weight", 100.0);
 
  // Recast double to float
  std::vector<double> deadband_velocities{0.0, 0.0, 0.0};

diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
@@ -649,10 +649,11 @@ TEST(CriticTests, VelocityDeadbandCritic)
  critic.score(data);
  EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
 
- // provide velocities inside deadband bounds, should have non-zero costs
+ // 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);
- EXPECT_GT(xt::sum(costs, immediate)(), 0.0f);
+ // 100.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
+ EXPECT_NEAR(costs(1), 56.7, 0.01);
 }