New MPPI Cost Critic (Contrib: Brice Renaudeau)

nav2_mppi_controller/CMakeLists.txt

@@ -79,6 +79,7 @@ add_library(mppi_controller SHARED
 
 add_library(mppi_critics SHARED
   src/critics/obstacles_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,20 @@ 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 3.81. Wight to apply to critic to avoid obstacles.                                       | 
+ | 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.                                          |
+ | 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 +230,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: 3.81
+      #   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,6 +5,10 @@
       <description>mppi critic for obstacle avoidance</description>
     </class>
 
+    <class type="mppi::critics::CostCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for obstacle avoidance using costmap score</description>
+    </class>
+
     <class type="mppi::critics::GoalCritic" base_class_type="mppi::critics::CriticFunction">
       <description>mppi critic for driving towards the goal</description>
     </class>

nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp

@@ -0,0 +1,98 @@
+// Copyright (c) 2023 Robocc Brice Renaudeau
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__COST_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"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::CostCritic
+ * @brief Critic objective function for avoiding obstacles using costmap's inflated cost
+ */
+class CostCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to obstacle avoidance
+   *
+   * @param costs [out] add obstacle cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  /**
+    * @brief Checks if cost represents a collision
+    * @param cost Point cost at pose center
+    * @param x X of pose
+    * @param y Y of pose
+    * @param theta theta of pose
+    * @return bool if in collision
+    */
+  bool inCollision(float cost, float x, float y, float theta);
+
+  /**
+    * @brief cost at a robot pose
+    * @param x X of pose
+    * @param y Y of pose
+    * @return Collision information at pose
+    */
+  float costAtPose(float x, float y);
+
+  /**
+    * @brief Find the min cost of the inflation decay function for which the robot MAY be
+    * in collision in any orientation
+    * @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)
+    * @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
+    */
+  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};
+  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__COST_CRITIC_HPP_

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -0,0 +1,200 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+
+namespace mppi::critics
+{
+
+void CostCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.81);
+  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(""));
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  collision_checker_.setCostmap(costmap_);
+  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possibly_inscribed_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float CostCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  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
+  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);
+  } else {
+    RCLCPP_WARN(
+      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!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+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)) {
+    near_goal = true;
+  }
+
+  auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
+  repulsive_cost.fill(0.0);
+
+  const size_t traj_len = data.trajectories.x.shape(1);
+  bool all_trajectories_collide = true;
+  for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
+    bool trajectory_collide = false;
+    const auto & traj = data.trajectories;
+    float pose_cost;
+
+    for (size_t j = 0; j < traj_len; j++) {
+      // The costAtPose doesn't use orientation
+      // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
+      // So the center point has more information than the footprint
+      pose_cost = costAtPose(traj.x(i, j), traj.y(i, j));
+      if (pose_cost < 1.0f) {continue;}  // In free space
+
+      if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) {
+        trajectory_collide = true;
+        break;
+      }
+
+      // Let near-collision trajectory points be punished severely
+      // Note that we collision check based on the footprint actual,
+      // but score based on the center-point cost regardless
+      using namespace nav2_costmap_2d; // NOLINT
+      if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) {
+        repulsive_cost[i] += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        repulsive_cost[i] += pose_cost;
+      }
+    }
+
+    if (!trajectory_collide) {
+      all_trajectories_collide = false;
+    } else {
+      repulsive_cost[i] = collision_cost_;
+    }
+  }
+
+  data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_);
+  data.fail_flag = all_trajectories_collide;
+}
+
+/**
+  * @brief Checks if cost represents a collision
+  * @param cost Costmap cost
+  * @return bool if in collision
+  */
+bool CostCritic::inCollision(float cost, float x, float y, float theta)
+{
+  bool is_tracking_unknown =
+    costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+
+  // If consider_footprint_ check footprint scort for collision
+  if (consider_footprint_ &&
+    (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+  {
+    cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+        x, y, theta, costmap_ros_->getRobotFootprint()));
+  }
+
+  switch (static_cast<unsigned char>(cost)) {
+    using namespace nav2_costmap_2d; // NOLINT
+    case (LETHAL_OBSTACLE):
+      return true;
+    case (INSCRIBED_INFLATED_OBSTACLE):
+      return consider_footprint_ ? false : true;
+    case (NO_INFORMATION):
+      return is_tracking_unknown ? false : true;
+  }
+
+  return false;
+}
+
+float CostCritic::costAtPose(float x, float y)
+{
+  using namespace nav2_costmap_2d;   // NOLINT
+  unsigned int x_i, y_i;
+  if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+    return nav2_costmap_2d::NO_INFORMATION;
+  }
+
+  return collision_checker_.pointCost(x_i, y_i);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)

nav2_mppi_controller/test/critics_tests.cpp

@@ -24,6 +24,7 @@
 #include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
 #include "nav2_mppi_controller/critics/goal_critic.hpp"
 #include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
 #include "nav2_mppi_controller/critics/path_align_critic.hpp"
 #include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp"
 #include "nav2_mppi_controller/critics/path_angle_critic.hpp"

nav2_mppi_controller/test/optimizer_smoke_test.cpp

@@ -103,7 +103,7 @@ INSTANTIATE_TEST_SUITE_P(
     std::make_tuple(
       "DiffDrive",
       std::vector<std::string>(
-        {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
+        {{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"},
           {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
       true),
     std::make_tuple(