New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090)

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>
ros-navigation · Apr 4, 2024 · 342ad51 · 342ad51
1 parent e1129b4
commit 342ad51
Show file tree

Hide file tree

Showing 7 changed files with 331 additions and 1 deletion.
diff --git a/nav2_mppi_controller/CMakeLists.txt b/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

diff --git a/nav2_mppi_controller/README.md b/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

diff --git a/nav2_mppi_controller/critics.xml b/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>

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/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_
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/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)
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/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"

diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/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(