Updated code to use getInflationLayer() method (ros-navigation#4076)

* updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]>
BriceRenaudeau · Jan 29, 2024 · fe2cb91 · fe2cb91
1 parent f3e8c2c
commit fe2cb91
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 34 deletions.
diff --git 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,35 +56,22 @@ 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
     return circumscribed_cost_;
   }
 
   // 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
@@ -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. "