Share code

Signed-off-by: Brice <[email protected]>

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/inflation_cost_critic.cpp
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
   src/critics/path_align_critic.cpp

nav2_mppi_controller/critics.xml

@@ -5,6 +5,10 @@
       <description>mppi critic for obstacle avoidance</description>
     </class>
 
+    <class type="mppi::critics::InflationCostCritic" 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/inflation_cost_critic.hpp

@@ -0,0 +1,79 @@
+#ifndef VEH_NAV_ACTION__CRITICS__OBSTACLES_CRITIC_HPP_
+#define VEH_NAV_ACTION__CRITICS__OBSTACLES_CRITIC_HPP_
+
+#include <memory>
+#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::InflationCostCritic
+ * @brief Critic objective function for avoiding obstacles using inflation cost
+ */
+class InflationCostCritic : 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 x X of pose
+    * @param y Y of pose
+    * @param theta theta of pose
+    * @return bool if in collision
+    */
+  bool inCollision(float x, float y, float theta);
+
+  /**
+    * @brief cost at a robot pose
+    * @param x X of pose
+    * @param y Y of pose
+    * @param theta theta of pose
+    * @return Collision information at pose
+    */
+  CollisionCost 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
+    */
+  double 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 repulsion_weight_{0};
+
+  float near_goal_distance_;
+
+  unsigned int power_{0};
+};
+
+}  // namespace mppi::critics
+
+#endif  // VEH_NAV_ACTION__CRITICS__OBSTACLES_CRITIC_HPP_

nav2_mppi_controller/src/critics/inflation_cost_critic.cpp

@@ -0,0 +1,197 @@
+// 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 "veh_nav_action/critics/inflation_cost_critic.hpp"
+
+namespace mppi::critics
+{
+
+void InflationCostCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(consider_footprint_, "consider_footprint", false);
+  getParam(power_, "cost_power", 1);
+  getParam(repulsion_weight_, "repulsion_weight", 1.5);
+  getParam(critical_cost_, "critical_cost", 300.0);
+  getParam(collision_cost_, "collision_cost", 10000.0);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5);
+
+  collision_checker_.setCostmap(costmap_);
+  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possibly_inscribed_cost_ < 1) {
+    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_, repulsion_weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+double InflationCostCritic::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;
+    }
+
+    inflation_layer_found = true;
+    const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+    const double resolution = costmap->getCostmap()->getResolution();
+    result = inflation_layer->computeCost(circum_radius / resolution);
+  }
+
+  if (!inflation_layer_found) {
+    RCLCPP_WARN(
+      rclcpp::get_logger("computeCircumscribedCost"),
+      "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;
+}
+
+void InflationCostCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_) {
+    return;
+  }
+
+  // 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;
+    CollisionCost 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.cost < 1) {continue;}  // In free space
+
+      if (inCollision(traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) {
+        trajectory_collide = true;
+        break;
+      }
+
+      // Let near-collision trajectory points be punished severely
+      using namespace nav2_costmap_2d; // NOLINT
+      if (pose_cost.cost >= INSCRIBED_INFLATED_OBSTACLE) {
+        repulsive_cost[i] += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        repulsive_cost[i] += pose_cost.cost;
+      }
+    }
+
+    if (!trajectory_collide) {
+      all_trajectories_collide = false;
+    } else {
+      repulsive_cost[i] = collision_cost_;
+    }
+  }
+
+  data.costs += xt::pow((repulsion_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 InflationCostCritic::inCollision(float x, float y, float theta)
+{
+  bool is_tracking_unknown =
+    costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+
+  unsigned int x_i, y_i;
+  collision_checker_.worldToMap(x, y, x_i, y_i);
+  double cost = collision_checker_.pointCost(x_i, y_i);
+
+  // If consider_footprint_ check footprint scort for collision
+  if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) {
+    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;
+}
+
+CollisionCost InflationCostCritic::costAtPose(float x, float y)
+{
+  using namespace nav2_costmap_2d;   // NOLINT
+  CollisionCost collision_cost;
+  float & cost = collision_cost.cost;
+  collision_cost.using_footprint = false;
+  unsigned int x_i, y_i;
+  if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+    cost = nav2_costmap_2d::NO_INFORMATION;
+    return collision_cost;
+  }
+  cost = collision_checker_.pointCost(x_i, y_i);
+
+  return collision_cost;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::InflationCostCritic,
+  mppi::critics::CriticFunction)