rename, add defaults, and docs

nav2_mppi_controller/CMakeLists.txt

@@ -79,7 +79,7 @@ add_library(mppi_controller SHARED
 
 add_library(mppi_critics SHARED
   src/critics/obstacles_critic.cpp
-  src/critics/inflation_cost_critic.cpp
+  src/critics/cost_critic.cpp
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
   src/critics/path_align_critic.cpp

nav2_mppi_controller/README.md

@@ -98,6 +98,9 @@ This process is then repeated a number of times and returns a converged solution
 
 
 #### Obstacles Critic
+
+Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles
+
  | Parameter            | Type   | Definition                                                                                                  |
  | ---------------      | ------ | ----------------------------------------------------------------------------------------------------------- |
  | consider_footprint   | bool   | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
@@ -109,6 +112,21 @@ 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.   
  | 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. |
 
+#### Cost Critic
+
+Uses inflated costmap cost directly to avoid obstacles
+
+ | Parameter            | Type   | Definition                                                                                                  |
+ | ---------------      | ------ | ----------------------------------------------------------------------------------------------------------- |
+ | consider_footprint   | bool   | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
+ | cost_weight          | double | Default 0.015. Wight to apply to critic to avoid obstacles. This is lower than others since it is normalized by cost range (ie 254 max cost, versus other critics using SI units)                                       | 
+ | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
+ | collision_cost       | double | Default 1000000.0. Cost to apply to a true collision in a trajectory.                                          |
+ | critical_cost       | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.                                          |
+ | collision_margin_distance   | double    | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. |
+ | 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.   
+ | 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                                                                                                                         |
  | ---------------            | ------ | -----------------------------------------------------------------------------------------------------------                        |
@@ -213,6 +231,15 @@ controller_server:
         collision_cost: 10000.0
         collision_margin_distance: 0.1
         near_goal_distance: 0.5
+      # Option to replace Obstacles and use Cost instead
+      # CostCritic:
+      #   enabled: true
+      #   cost_power: 1
+      #   cost_weight: 0.015
+      #   critical_cost: 300.0
+      #   consider_footprint: true
+      #   collision_cost: 1000000.0
+      #   near_goal_distance: 1.0
       PathAlignCritic:
         enabled: true
         cost_power: 1

nav2_mppi_controller/critics.xml

@@ -5,7 +5,7 @@
       <description>mppi critic for obstacle avoidance</description>
     </class>
 
-    <class type="mppi::critics::InflationCostCritic" base_class_type="mppi::critics::CriticFunction">
+    <class type="mppi::critics::CostCritic" base_class_type="mppi::critics::CriticFunction">
       <description>mppi critic for obstacle avoidance using costmap score</description>
     </class>
 

nav2_mppi_controller/include/nav2_mppi_controller/critics/inflation_cost_critic.hpp → nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_MPPI_CONTROLLER__CRITICS__INFLATION_COST_CRITIC_HPP_
-#define NAV2_MPPI_CONTROLLER__CRITICS__INFLATION_COST_CRITIC_HPP_
+#ifndef NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
 
 #include <memory>
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
@@ -27,10 +27,10 @@ namespace mppi::critics
 {
 
 /**
- * @class mppi::critics::InflationCostCritic
- * @brief Critic objective function for avoiding obstacles using inflation cost
+ * @class mppi::critics::CostCritic
+ * @brief Critic objective function for avoiding obstacles using costmap's inflated cost
  */
-class InflationCostCritic : public CriticFunction
+class CostCritic : public CriticFunction
 {
 public:
   /**
@@ -72,23 +72,26 @@ class InflationCostCritic : public CriticFunction
     * @return double circumscribed cost, any higher than this and need to do full footprint collision checking
     * since some element of the robot could be in collision
     */
-  double findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
+  float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
 
 protected:
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
   collision_checker_{nullptr};
   float possibly_inscribed_cost_;
 
   bool consider_footprint_{true};
-  double collision_cost_{0};
-  double critical_cost_{0};
-  double weight_{0};
+  float circumscribed_radius_{0};
+  float circumscribed_cost_{0};
+  float collision_cost_{0};
+  float critical_cost_{0};
+  float weight_{0};
 
   float near_goal_distance_;
+  std::string inflation_layer_name_;
 
   unsigned int power_{0};
 };
 
 }  // namespace mppi::critics
 
-#endif  // NAV2_MPPI_CONTROLLER__CRITICS__INFLATION_COST_CRITIC_HPP_
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_

nav2_mppi_controller/src/critics/inflation_cost_critic.cpp → nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -14,12 +14,12 @@
 // limitations under the License.
 
 #include <cmath>
-#include "nav2_mppi_controller/critics/inflation_cost_critic.hpp"
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
 
 namespace mppi::critics
 {
 
-void InflationCostCritic::initialize()
+void CostCritic::initialize()
 {
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
@@ -28,6 +28,7 @@ void InflationCostCritic::initialize()
   getParam(critical_cost_, "critical_cost", 300.0);
   getParam(collision_cost_, "collision_cost", 1000000.0);
   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_);
@@ -50,47 +51,51 @@ void InflationCostCritic::initialize()
     "footprint" : "circular");
 }
 
-double InflationCostCritic::findCircumscribedCost(
+float CostCritic::findCircumscribedCost(
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
 {
   double result = -1.0;
-  bool inflation_layer_found = false;
-  // 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) {
-      continue;
-    }
+  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_;
+  }
 
-    inflation_layer_found = true;
-    const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  // 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 double resolution = costmap->getCostmap()->getResolution();
     result = inflation_layer->computeCost(circum_radius / resolution);
-  }
-
-  if (!inflation_layer_found) {
+  } else {
     RCLCPP_WARN(
-      rclcpp::get_logger("computeCircumscribedCost"),
+      logger_,
       "No inflation layer found in costmap configuration. "
       "If this is an SE2-collision checking plugin, it cannot use costmap potential "
       "field to speed up collision checking by only checking the full footprint "
       "when robot is within possibly-inscribed radius of an obstacle. This may "
       "significantly slow down planning times and not avoid anything but absolute collisions!");
   }
 
-  return result;
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
 }
 
-void InflationCostCritic::score(CriticData & data)
+void CostCritic::score(CriticData & data)
 {
   using xt::evaluation_strategy::immediate;
   if (!enabled_) {
     return;
   }
 
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
   if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
@@ -144,7 +149,7 @@ void InflationCostCritic::score(CriticData & data)
   * @param cost Costmap cost
   * @return bool if in collision
   */
-bool InflationCostCritic::inCollision(float cost, float x, float y, float theta)
+bool CostCritic::inCollision(float cost, float x, float y, float theta)
 {
   bool is_tracking_unknown =
     costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
@@ -170,7 +175,7 @@ bool InflationCostCritic::inCollision(float cost, float x, float y, float theta)
   return false;
 }
 
-float InflationCostCritic::costAtPose(float x, float y)
+float CostCritic::costAtPose(float x, float y)
 {
   using namespace nav2_costmap_2d;   // NOLINT
   unsigned int x_i, y_i;
@@ -186,5 +191,5 @@ float InflationCostCritic::costAtPose(float x, float y)
 #include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(
-  mppi::critics::InflationCostCritic,
+  mppi::critics::CostCritic,
   mppi::critics::CriticFunction)