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 index 77df20adcb..14c0fe93f4 100644 --- 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 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 index 358994c238..31b6a3d0df 100644 --- 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 index 137e943376..7a9b7729ed 100644 --- 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(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 index 21d8239615..cdf0113564 100644 --- 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(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 index 53e019a3ae..205c3f0471 100644 --- 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 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 index 918a8a9b4e..2f23442097 100644 --- 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(possible_inscribed_cost); + possible_collision_cost_ = static_cast(possible_collision_cost); footprint_is_radius_ = radius; // Use radius, no caching required @@ -113,8 +113,8 @@ bool GridCollisionChecker::inCollision( footprint_cost_ = static_cast(costmap_->getCost( static_cast(x + 0.5), static_cast(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(