Refactors possible_inscribed_cost to possible_circumscribed_cost …

…in collision checker for smac_planner and mppi_controller (ros-navigation#4113). (ros-navigation#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue <[email protected]> Signed-off-by: enricosutera <[email protected]>
enricosutera · May 19, 2024 · 034b833 · 034b833
1 parent fa3b4d4
commit 034b833
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 16 deletions.
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
@@ -78,7 +78,7 @@ class CostCritic : public CriticFunction
 protected:
   nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
   collision_checker_{nullptr};
-  float possibly_inscribed_cost_;
+  float possible_collision_cost_;
 
   bool consider_footprint_{true};
   float circumscribed_radius_{0};

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
@@ -90,7 +90,7 @@ class ObstaclesCritic : public CriticFunction
   float collision_cost_{0};
   float inflation_scale_factor_{0}, inflation_radius_{0};
 
-  float possibly_inscribed_cost_;
+  float possible_collision_cost_;
   float collision_margin_distance_;
   float near_goal_distance_;
   float circumscribed_cost_{0}, circumscribed_radius_{0};

diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -34,9 +34,9 @@ void CostCritic::initialize()
   weight_ /= 254.0f;
 
   collision_checker_.setCostmap(costmap_);
-  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
 
-  if (possibly_inscribed_cost_ < 1.0f) {
+  if (possible_collision_cost_ < 1.0f) {
     RCLCPP_ERROR(
       logger_,
       "Inflation layer either not found or inflation is not set sufficiently for "
@@ -96,7 +96,7 @@ void CostCritic::score(CriticData & data)
 
   if (consider_footprint_) {
     // footprint may have changed since initialization if user has dynamic footprints
-    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
   }
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
@@ -161,7 +161,7 @@ bool CostCritic::inCollision(float cost, float x, float y, float theta)
 
   // If consider_footprint_ check footprint scort for collision
   if (consider_footprint_ &&
-    (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+    (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
   {
     cost = static_cast<float>(collision_checker_.footprintCostAtPose(
         x, y, theta, costmap_ros_->getRobotFootprint()));

diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -32,9 +32,9 @@ void ObstaclesCritic::initialize()
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
 
   collision_checker_.setCostmap(costmap_);
-  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
 
-  if (possibly_inscribed_cost_ < 1.0f) {
+  if (possible_collision_cost_ < 1.0f) {
     RCLCPP_ERROR(
       logger_,
       "Inflation layer either not found or inflation is not set sufficiently for "
@@ -111,7 +111,7 @@ void ObstaclesCritic::score(CriticData & data)
 
   if (consider_footprint_) {
     // footprint may have changed since initialization if user has dynamic footprints
-    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+    possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
   }
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
@@ -212,7 +212,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
   cost = collision_checker_.pointCost(x_i, y_i);
 
   if (consider_footprint_ &&
-    (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+    (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
   {
     cost = static_cast<float>(collision_checker_.footprintCostAtPose(
         x, y, theta, costmap_ros_->getRobotFootprint()));

diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp
@@ -66,7 +66,7 @@ class GridCollisionChecker
   void setFootprint(
     const nav2_costmap_2d::Footprint & footprint,
     const bool & radius,
-    const double & possible_inscribed_cost);
+    const double & possible_collision_cost);
 
   /**
    * @brief Check if in collision with costmap and footprint at pose
@@ -130,7 +130,7 @@ class GridCollisionChecker
   float footprint_cost_;
   bool footprint_is_radius_;
   std::vector<float> angles_;
-  float possible_inscribed_cost_{-1};
+  float possible_collision_cost_{-1};
   rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
   rclcpp::Clock::SharedPtr clock_;
 };

diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp
@@ -51,9 +51,9 @@ GridCollisionChecker::GridCollisionChecker(
 void GridCollisionChecker::setFootprint(
   const nav2_costmap_2d::Footprint & footprint,
   const bool & radius,
-  const double & possible_inscribed_cost)
+  const double & possible_collision_cost)
 {
-  possible_inscribed_cost_ = static_cast<float>(possible_inscribed_cost);
+  possible_collision_cost_ = static_cast<float>(possible_collision_cost);
   footprint_is_radius_ = radius;
 
   // Use radius, no caching required
@@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision(
     footprint_cost_ = static_cast<float>(costmap_->getCost(
         static_cast<unsigned int>(x + 0.5), static_cast<unsigned int>(y + 0.5)));
 
-    if (footprint_cost_ < possible_inscribed_cost_) {
-      if (possible_inscribed_cost_ > 0) {
+    if (footprint_cost_ < possible_collision_cost_) {
+      if (possible_collision_cost_ > 0) {
         return false;
       } else {
         RCLCPP_ERROR_THROTTLE(