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 5b31ee87cd..0f31860f5a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -127,7 +127,7 @@ class GridCollisionChecker std::shared_ptr costmap_ros_; std::vector oriented_footprints_; nav2_costmap_2d::Footprint unoriented_footprint_; - float footprint_cost_; + float center_cost_; bool footprint_is_radius_; std::vector angles_; float possible_collision_cost_{-1}; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index e8bf083e64..7d2684d33d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -436,14 +436,6 @@ class NodeHybrid const unsigned int & start_x, const unsigned int & start_y, const unsigned int & goal_x, const unsigned int & goal_y); - /** - * @brief Using the inflation layer, find the footprint's adjusted cost - * if the robot is non-circular - * @param cost Cost to adjust - * @return float Cost adjusted - */ - static float adjustedFootprintCost(const float & cost); - /** * @brief Retrieve all valid neighbors of a node. * @param validity_checker Functor for state validity checking @@ -471,7 +463,6 @@ class NodeHybrid */ static void destroyStaticAssets() { - inflation_layer.reset(); costmap_ros.reset(); } @@ -486,7 +477,6 @@ class NodeHybrid static ObstacleHeuristicQueue obstacle_heuristic_queue; static std::shared_ptr costmap_ros; - static std::shared_ptr inflation_layer; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 5afb919662..98910084d6 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -115,32 +115,31 @@ bool GridCollisionChecker::inCollision( } // Assumes setFootprint already set - double wx, wy; - costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); + center_cost_ = static_cast(costmap_->getCost( + static_cast(x + 0.5f), static_cast(y + 0.5f))); if (!footprint_is_radius_) { // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision - footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (footprint_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { + if (center_cost_ < possible_collision_cost_ && possible_collision_cost_ > 0.0f) { return false; } // If its inscribed, in collision, or unknown in the middle, // no need to even check the footprint, its invalid - if (footprint_cost_ == UNKNOWN_COST && !traverse_unknown) { + if (center_cost_ == UNKNOWN_COST && !traverse_unknown) { return true; } - if (footprint_cost_ == INSCRIBED_COST || footprint_cost_ == OCCUPIED_COST) { + if (center_cost_ == INSCRIBED_COST || center_cost_ == OCCUPIED_COST) { return true; } // if possible inscribed, need to check actual footprint pose. // Use precomputed oriented footprints are done on initialization, // offset by translation value to collision check + double wx, wy; + costmap_->mapToWorld(static_cast(x), static_cast(y), wx, wy); geometry_msgs::msg::Point new_pt; const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin]; nav2_costmap_2d::Footprint current_footprint; @@ -151,25 +150,22 @@ bool GridCollisionChecker::inCollision( current_footprint.push_back(new_pt); } - footprint_cost_ = static_cast(footprintCost(current_footprint)); + float footprint_cost = static_cast(footprintCost(current_footprint)); - if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { + if (footprint_cost == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= OCCUPIED_COST; + return footprint_cost >= OCCUPIED_COST; } else { // if radius, then we can check the center of the cost assuming inflation is used - footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5f), static_cast(y + 0.5f))); - - if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED_COST; + return center_cost_ >= INSCRIBED_COST; } } @@ -177,19 +173,19 @@ bool GridCollisionChecker::inCollision( const unsigned int & i, const bool & traverse_unknown) { - footprint_cost_ = costmap_->getCost(i); - if (footprint_cost_ == UNKNOWN_COST && traverse_unknown) { + center_cost_ = costmap_->getCost(i); + if (center_cost_ == UNKNOWN_COST && traverse_unknown) { return false; } // if occupied or unknown and not to traverse unknown space - return footprint_cost_ >= INSCRIBED_COST; + return center_cost_ >= INSCRIBED_COST; } float GridCollisionChecker::getCost() { // Assumes inCollision called prior - return static_cast(footprint_cost_); + return static_cast(center_cost_); } bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index c1a7a23607..abbdc6f712 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -41,7 +41,6 @@ HybridMotionTable NodeHybrid::motion_table; float NodeHybrid::size_lookup = 25; LookupTable NodeHybrid::dist_heuristic_lookup_table; std::shared_ptr NodeHybrid::costmap_ros = nullptr; -std::shared_ptr NodeHybrid::inflation_layer = nullptr; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; @@ -492,7 +491,6 @@ void NodeHybrid::resetObstacleHeuristic( // erosion of path quality after even modest smoothing. The error would be no more // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality costmap_ros = costmap_ros_i; - inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros); auto costmap = costmap_ros->getCostmap(); // Clear lookup table @@ -539,29 +537,6 @@ void NodeHybrid::resetObstacleHeuristic( obstacle_heuristic_lookup_table[goal_index] = -0.00001f; } -float NodeHybrid::adjustedFootprintCost(const float & cost) -{ - if (!inflation_layer) { - return cost; - } - - const auto layered_costmap = costmap_ros->getLayeredCostmap(); - const float scale_factor = inflation_layer->getCostScalingFactor(); - const float min_radius = layered_costmap->getInscribedRadius(); - float dist_to_obj = (scale_factor * min_radius - log(cost) + log(253.0f)) / scale_factor; - - // Subtract minimum radius for edge cost - dist_to_obj -= min_radius; - if (dist_to_obj < 0.0f) { - dist_to_obj = 0.0f; - } - - // Compute cost at this value - return static_cast( - inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution())); -} - - float NodeHybrid::getObstacleHeuristic( const Coordinates & node_coords, const Coordinates &, @@ -569,7 +544,6 @@ float NodeHybrid::getObstacleHeuristic( { // If already expanded, return the cost auto costmap = costmap_ros->getCostmap(); - const bool is_circular = costmap_ros->getUseRadius(); unsigned int size_x = 0u; unsigned int size_y = 0u; if (motion_table.downsample_obstacle_heuristic) { @@ -671,13 +645,7 @@ float NodeHybrid::getObstacleHeuristic( cost = static_cast(costmap->getCost(new_idx)); } - if (!is_circular) { - // Adjust cost value if using SE2 footprint checks - cost = adjustedFootprintCost(cost); - if (cost >= OCCUPIED_COST) { - continue; - } - } else if (cost >= INSCRIBED_COST) { + if (cost >= INSCRIBED_COST) { continue; }