From 182414c618a2d4edc97dd620bbb657aff342dc40 Mon Sep 17 00:00:00 2001
From: jncfa <20467009+jncfa@users.noreply.github.com>
Date: Sat, 27 Jan 2024 00:56:51 +0100
Subject: [PATCH] Updated code to use getInflationLayer() method (#4076)

* updated code to use getInflationLayer method

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>

* Fix linting

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>

---------

Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com>
---
 .../src/critics/obstacles_critic.cpp          | 25 +++++--------------
 .../include/nav2_smac_planner/utils.hpp       | 18 +++----------
 2 files changed, 9 insertions(+), 34 deletions(-)

diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
index ecb22b295b..21d8239615 100644
--- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp
+++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -15,7 +15,7 @@
 
 #include <cmath>
 #include "nav2_mppi_controller/critics/obstacles_critic.hpp"
-
+#include "nav2_costmap_2d/inflation_layer.hpp"
 namespace mppi::critics
 {
 
@@ -56,8 +56,6 @@ float ObstaclesCritic::findCircumscribedCost(
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
 {
   double result = -1.0;
-  bool inflation_layer_found = false;
-
   const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
   if (static_cast<float>(circum_radius) == circumscribed_radius_) {
     // early return if footprint size is unchanged
@@ -65,26 +63,15 @@ float ObstaclesCritic::findCircumscribedCost(
   }
 
   // check if the costmap has an inflation layer
-  for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
-    layer != costmap->getLayeredCostmap()->getPlugins()->end();
-    ++layer)
-  {
-    auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
-    if (!inflation_layer ||
-      (!inflation_layer_name_.empty() &&
-      inflation_layer->getName() != inflation_layer_name_))
-    {
-      continue;
-    }
-
-    inflation_layer_found = true;
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(
+    costmap,
+    inflation_layer_name_);
+  if (inflation_layer != nullptr) {
     const double resolution = costmap->getCostmap()->getResolution();
     result = inflation_layer->computeCost(circum_radius / resolution);
     inflation_scale_factor_ = static_cast<float>(inflation_layer->getCostScalingFactor());
     inflation_radius_ = static_cast<float>(inflation_layer->getInflationRadius());
-  }
-
-  if (!inflation_layer_found) {
+  } else {
     RCLCPP_WARN(
       logger_,
       "No inflation layer found in costmap configuration. "
diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
index 277edb6083..86f0b8671a 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp
@@ -76,27 +76,15 @@ inline geometry_msgs::msg::Quaternion getWorldOrientation(
 inline double findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
 {
   double result = -1.0;
-  bool inflation_layer_found = false;
   std::vector<std::shared_ptr<nav2_costmap_2d::Layer>>::iterator layer;
 
   // check if the costmap has an inflation layer
-  for (layer = costmap->getLayeredCostmap()->getPlugins()->begin();
-    layer != costmap->getLayeredCostmap()->getPlugins()->end();
-    ++layer)
-  {
-    std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer =
-      std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
-    if (!inflation_layer) {
-      continue;
-    }
-
-    inflation_layer_found = true;
+  const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap);
+  if (inflation_layer != nullptr) {
     double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
     double resolution = costmap->getCostmap()->getResolution();
     result = static_cast<double>(inflation_layer->computeCost(circum_radius / resolution));
-  }
-
-  if (!inflation_layer_found) {
+  } else {
     RCLCPP_WARN(
       rclcpp::get_logger("computeCircumscribedCost"),
       "No inflation layer found in costmap configuration. "