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

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#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 <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#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 <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#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 <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * 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 <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * 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 <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * 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 <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * 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 <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * 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 <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> Signed-off-by: enricosutera <[email protected]>
enricosutera · May 19, 2024 · 2241d1f · 2241d1f
1 parent e5da1f9
commit 2241d1f
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(