diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 893ed426a56..15f6fa6c729 100644 --- 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 index 9b22bf77d3a..76e1dbd670f 100644 --- 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 + #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 index e5e6ef66331..d2b60746f88 100644 --- 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 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 index cba9c312175..be24393bfe7 100644 --- 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({1000, 30}); state.vy = 0.02 * xt::ones({1000, 30}); state.wz = 0.021 * xt::ones({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); }