Add inflation_layer_name param (ros-navigation#4047)

Signed-off-by: Renan Salles <[email protected]>
(cherry picked from commit 29321d6)

nav2_mppi_controller/README.md

@@ -110,6 +110,7 @@ This process is then repeated a number of times and returns a converged solution
  | near_goal_distance          | double    | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.   
  | cost_scaling_factor       | double    | Default 10.0. Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only)
  | inflation_radius          | double    | Default 0.55. Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only)
+ | inflation_layer_name        | string    | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
 
 #### Path Align Critic
  | Parameter                  | Type   | Definition                                                                                                                         |

nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp

@@ -16,9 +16,10 @@
 #define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
 
 #include <memory>
+#include <string>
+
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
-
 #include "nav2_mppi_controller/critic_function.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
@@ -96,6 +97,7 @@ class ObstaclesCritic : public CriticFunction
 
   unsigned int power_{0};
   float repulsion_weight_, critical_weight_{0};
+  std::string inflation_layer_name_;
 };
 
 }  // namespace mppi::critics

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -28,6 +28,7 @@ void ObstaclesCritic::initialize()
   getParam(collision_cost_, "collision_cost", 10000.0);
   getParam(collision_margin_distance_, "collision_margin_distance", 0.10);
   getParam(near_goal_distance_, "near_goal_distance", 0.5);
+  getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
 
   collision_checker_.setCostmap(costmap_);
   possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
@@ -68,7 +69,10 @@ float ObstaclesCritic::findCircumscribedCost(
     ++layer)
   {
     auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
-    if (!inflation_layer) {
+    if (!inflation_layer ||
+      (!inflation_layer_name_.empty() &&
+      inflation_layer->getName() != inflation_layer_name_))
+    {
       continue;
     }