From 9d7d567dbf98f5b70a4a457fe80968b45276b088 Mon Sep 17 00:00:00 2001 From: Jose Faria <20467009+jncfa@users.noreply.github.com> Date: Fri, 26 Jan 2024 22:57:51 +0100 Subject: [PATCH 1/2] updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --- .../src/critics/obstacles_critic.cpp | 22 ++++--------------- .../include/nav2_smac_planner/utils.hpp | 17 +++----------- 2 files changed, 7 insertions(+), 32 deletions(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index ecb22b295b..bb66dbd22e 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -15,7 +15,7 @@ #include #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 costmap) { double result = -1.0; - bool inflation_layer_found = false; - const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); if (static_cast(circum_radius) == circumscribed_radius_) { // early return if footprint size is unchanged @@ -65,26 +63,14 @@ 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(*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(inflation_layer->getCostScalingFactor()); inflation_radius_ = static_cast(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..3446019acb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -76,27 +76,16 @@ inline geometry_msgs::msg::Quaternion getWorldOrientation( inline double findCircumscribedCost(std::shared_ptr costmap) { double result = -1.0; - bool inflation_layer_found = false; std::vector>::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 inflation_layer = - std::dynamic_pointer_cast(*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(inflation_layer->computeCost(circum_radius / resolution)); } - - if (!inflation_layer_found) { + else{ RCLCPP_WARN( rclcpp::get_logger("computeCircumscribedCost"), "No inflation layer found in costmap configuration. " From 22c2a15d6f42d5bbf537da05a959021057d0fe14 Mon Sep 17 00:00:00 2001 From: Jose Faria <20467009+jncfa@users.noreply.github.com> Date: Fri, 26 Jan 2024 23:15:00 +0100 Subject: [PATCH 2/2] Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 9 +++++---- nav2_smac_planner/include/nav2_smac_planner/utils.hpp | 5 ++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index bb66dbd22e..21d8239615 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -63,14 +63,15 @@ float ObstaclesCritic::findCircumscribedCost( } // check if the costmap has an inflation layer - const auto inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap, inflation_layer_name_); - if (inflation_layer != nullptr){ + 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(inflation_layer->getCostScalingFactor()); inflation_radius_ = static_cast(inflation_layer->getInflationRadius()); - } - else{ + } 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 3446019acb..86f0b8671a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -80,12 +80,11 @@ inline double findCircumscribedCost(std::shared_ptrgetLayeredCostmap()->getCircumscribedRadius(); double resolution = costmap->getCostmap()->getResolution(); result = static_cast(inflation_layer->computeCost(circum_radius / resolution)); - } - else{ + } else { RCLCPP_WARN( rclcpp::get_logger("computeCircumscribedCost"), "No inflation layer found in costmap configuration. "