[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Chec…

…king (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: enricosutera <[email protected]>

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -41,10 +41,13 @@
 #include <map>
 #include <vector>
 #include <mutex>
+#include <memory>
+#include <string>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -161,6 +164,25 @@ class InflationLayer : public Layer
     return cost;
   }
 
+  static std::shared_ptr<nav2_costmap_2d::InflationLayer> getInflationLayer(
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros,
+    const std::string layer_name = "")
+  {
+    const auto layered_costmap = costmap_ros->getLayeredCostmap();
+    for (auto layer = layered_costmap->getPlugins()->begin();
+      layer != layered_costmap->getPlugins()->end();
+      ++layer)
+    {
+      auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
+      if (inflation_layer) {
+        if (layer_name.empty() || inflation_layer->getName() == layer_name) {
+          return inflation_layer;
+        }
+      }
+    }
+    return nullptr;
+  }
+
   // Provide a typedef to ease future code maintenance
   typedef std::recursive_mutex mutex_t;
 

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -11,8 +11,12 @@
 // 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. Reserved.
+
 #include <vector>
+#include <memory>
+
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "nav2_smac_planner/constants.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -39,7 +43,7 @@ class GridCollisionChecker
    * orientations for to speed up collision checking
    */
   GridCollisionChecker(
-    nav2_costmap_2d::Costmap2D * costmap,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
     unsigned int num_quantizations,
     rclcpp_lifecycle::LifecycleNode::SharedPtr node);
 
@@ -103,6 +107,12 @@ class GridCollisionChecker
     return angles_;
   }
 
+  /**
+   * @brief Get costmap ros object for inflation layer params
+   * @return Costmap ros
+   */
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getCostmapROS() {return costmap_ros_;}
+
 private:
   /**
    * @brief Check if value outside the range
@@ -114,6 +124,7 @@ class GridCollisionChecker
   bool outsideRange(const unsigned int & max, const float & value);
 
 protected:
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
   nav2_costmap_2d::Footprint unoriented_footprint_;
   float footprint_cost_;

nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp

@@ -223,13 +223,11 @@ class Node2D
    * @brief Get cost of heuristic of node
    * @param node Node index current
    * @param node Node index of new
-   * @param costmap Costmap ptr to use
    * @return Heuristic cost between the nodes
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates,
-    const nav2_costmap_2d::Costmap2D * costmap);
+    const Coordinates & goal_coordinates);
 
   /**
    * @brief Initialize the neighborhood to be used in A*

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -31,6 +31,8 @@
 #include "nav2_smac_planner/types.hpp"
 #include "nav2_smac_planner/collision_checker.hpp"
 #include "nav2_smac_planner/costmap_downsampler.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_costmap_2d/inflation_layer.hpp"
 
 namespace nav2_smac_planner
 {
@@ -360,13 +362,11 @@ class NodeHybrid
    * @brief Get cost of heuristic of node
    * @param node Node index current
    * @param node Node index of new
-   * @param costmap Costmap ptr to use
    * @return Heuristic cost between the nodes
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates,
-    const nav2_costmap_2d::Costmap2D * costmap);
+    const Coordinates & goal_coordinates);
 
   /**
    * @brief Initialize motion models
@@ -423,14 +423,22 @@ class NodeHybrid
 
   /**
    * @brief reset the obstacle heuristic state
-   * @param costmap Costmap to use
+   * @param costmap_ros Costmap to use
    * @param goal_coords Coordinates to start heuristic expansion at
    */
   static void resetObstacleHeuristic(
-    nav2_costmap_2d::Costmap2D * costmap,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
     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
@@ -462,6 +470,8 @@ class NodeHybrid
   static ObstacleHeuristicQueue obstacle_heuristic_queue;
 
   static nav2_costmap_2d::Costmap2D * sampled_costmap;
+  static std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros;
+  static std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer;
   static CostmapDownsampler downsampler;
   // Dubin / Reeds-Shepp lookup and size for dereferencing
   static LookupTable dist_heuristic_lookup_table;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -312,13 +312,11 @@ class NodeLattice
    * @brief Get cost of heuristic of node
    * @param node Node index current
    * @param node Node index of new
-   * @param costmap Costmap ptr to use
    * @return Heuristic cost between the nodes
    */
   static float getHeuristicCost(
     const Coordinates & node_coords,
-    const Coordinates & goal_coordinates,
-    const nav2_costmap_2d::Costmap2D * costmap);
+    const Coordinates & goal_coordinates);
 
   /**
    * @brief Initialize motion models
@@ -355,12 +353,12 @@ class NodeLattice
    * @param goal_coords Coordinates to start heuristic expansion at
    */
   static void resetObstacleHeuristic(
-    nav2_costmap_2d::Costmap2D * costmap,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
     const unsigned int & start_x, const unsigned int & start_y,
     const unsigned int & goal_x, const unsigned int & goal_y)
   {
     // State Lattice and Hybrid-A* share this heuristics
-    NodeHybrid::resetObstacleHeuristic(costmap, start_x, start_y, goal_x, goal_y);
+    NodeHybrid::resetObstacleHeuristic(costmap_ros, start_x, start_y, goal_x, goal_y);
   }
 
   /**

nav2_smac_planner/src/a_star.cpp

@@ -207,7 +207,8 @@ void AStarAlgorithm<NodeT>::setGoal(
       throw std::runtime_error("Start must be set before goal.");
     }
 
-    NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my);
+    NodeT::resetObstacleHeuristic(
+      _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my);
   }
 
   _goal_coordinates = goal_coords;
@@ -403,7 +404,7 @@ float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
   const Coordinates node_coords =
     NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
   float heuristic = NodeT::getHeuristicCost(
-    node_coords, _goal_coordinates, _costmap);
+    node_coords, _goal_coordinates);
 
   if (heuristic < _best_heuristic_node.first) {
     _best_heuristic_node = {heuristic, node->getIndex()};

nav2_smac_planner/src/analytic_expansion.cpp

@@ -57,12 +57,12 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     _motion_model == MotionModel::STATE_LATTICE)
   {
     // See if we are closer and should be expanding more often
-    auto costmap = _collision_checker->getCostmap();
     const Coordinates node_coords =
-      NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size);
+      NodeT::getCoords(
+      current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
     closest_distance = std::min(
       closest_distance,
-      static_cast<int>(NodeT::getHeuristicCost(node_coords, goal_node->pose, costmap)));
+      static_cast<int>(NodeT::getHeuristicCost(node_coords, goal_node->pose)));
 
     // We want to expand at a rate of d/expansion_ratio,
     // but check to see if we are so close that we would be expanding every iteration

nav2_smac_planner/src/collision_checker.cpp

@@ -18,16 +18,20 @@ namespace nav2_smac_planner
 {
 
 GridCollisionChecker::GridCollisionChecker(
-  nav2_costmap_2d::Costmap2D * costmap,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
   unsigned int num_quantizations,
   rclcpp_lifecycle::LifecycleNode::SharedPtr node)
-: FootprintCollisionChecker(costmap)
+: FootprintCollisionChecker(costmap_ros ? costmap_ros->getCostmap() : nullptr)
 {
   if (node) {
     clock_ = node->get_clock();
     logger_ = node->get_logger();
   }
 
+  if (costmap_ros) {
+    costmap_ros_ = costmap_ros;
+  }
+
   // Convert number of regular bins into angles
   float bin_size = 2 * M_PI / static_cast<float>(num_quantizations);
   angles_.reserve(num_quantizations);

nav2_smac_planner/src/node_2d.cpp

@@ -81,8 +81,7 @@ float Node2D::getTraversalCost(const NodePtr & child)
 
 float Node2D::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coordinates,
-  const nav2_costmap_2d::Costmap2D * /*costmap*/)
+  const Coordinates & goal_coordinates)
 {
   // Using Moore distance as it more accurately represents the distances
   // even a Van Neumann neighborhood robot can navigate.

nav2_smac_planner/src/node_hybrid.cpp

@@ -41,6 +41,9 @@ HybridMotionTable NodeHybrid::motion_table;
 float NodeHybrid::size_lookup = 25;
 LookupTable NodeHybrid::dist_heuristic_lookup_table;
 nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
+std::shared_ptr<nav2_costmap_2d::Costmap2DROS> NodeHybrid::costmap_ros = nullptr;
+std::shared_ptr<nav2_costmap_2d::InflationLayer> NodeHybrid::inflation_layer = nullptr;
+
 CostmapDownsampler NodeHybrid::downsampler;
 ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
 
@@ -433,8 +436,7 @@ float NodeHybrid::getTraversalCost(const NodePtr & child)
 
 float NodeHybrid::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords,
-  const nav2_costmap_2d::Costmap2D * /*costmap*/)
+  const Coordinates & goal_coords)
 {
   const float obstacle_heuristic =
     getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty);
@@ -477,18 +479,20 @@ inline float distanceHeuristic2D(
 }
 
 void NodeHybrid::resetObstacleHeuristic(
-  nav2_costmap_2d::Costmap2D * costmap,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_i,
   const unsigned int & start_x, const unsigned int & start_y,
   const unsigned int & goal_x, const unsigned int & goal_y)
 {
   // Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up
   // the planner considerably to search through 75% less cells with no detectable
   // 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
-  sampled_costmap = costmap;
+  costmap_ros = costmap_ros_i;
+  inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros);
+  sampled_costmap = costmap_ros->getCostmap();
   if (motion_table.downsample_obstacle_heuristic) {
     std::weak_ptr<nav2_util::LifecycleNode> ptr;
-    downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0, true);
+    downsampler.on_configure(ptr, "fake_frame", "fake_topic", sampled_costmap, 2.0, true);
     downsampler.on_activate();
     sampled_costmap = downsampler.downsample(2.0);
   }
@@ -529,13 +533,37 @@ 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<float>(
+    inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution()));
+}
+
+
 float NodeHybrid::getObstacleHeuristic(
   const Coordinates & node_coords,
   const Coordinates & goal_coords,
   const double & cost_penalty)
 {
   // If already expanded, return the cost
   const unsigned int size_x = sampled_costmap->getSizeInCellsX();
+  const bool is_circular = costmap_ros->getUseRadius();
 
   // Divided by 2 due to downsampled costmap.
   unsigned int start_y, start_x;
@@ -605,7 +633,14 @@ float NodeHybrid::getObstacleHeuristic(
       // if neighbor path is better and non-lethal, set new cost and add to queue
       if (new_idx < size_x * size_y) {
         cost = static_cast<float>(sampled_costmap->getCost(new_idx));
-        if (cost >= INSCRIBED) {
+
+        if (!is_circular) {
+          // Adjust cost value if using SE2 footprint checks
+          cost = adjustedFootprintCost(cost);
+          if (cost >= OCCUPIED) {
+            continue;
+          }
+        } else if (cost >= INSCRIBED) {
           continue;
         }
 

nav2_smac_planner/src/node_lattice.cpp

@@ -327,8 +327,7 @@ float NodeLattice::getTraversalCost(const NodePtr & child)
 
 float NodeLattice::getHeuristicCost(
   const Coordinates & node_coords,
-  const Coordinates & goal_coords,
-  const nav2_costmap_2d::Costmap2D * /*costmap*/)
+  const Coordinates & goal_coords)
 {
   // get obstacle heuristic value
   const float obstacle_heuristic = getObstacleHeuristic(

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -108,7 +108,7 @@ void SmacPlanner2D::configure(
   }
 
   // Initialize collision checker
-  _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node);
+  _collision_checker = GridCollisionChecker(costmap_ros, 1 /*for 2D, most be 1*/, node);
   _collision_checker.setFootprint(
     costmap_ros->getRobotFootprint(),
     true /*for 2D, most use radius*/,

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -36,6 +36,7 @@ SmacPlannerHybrid::SmacPlannerHybrid()
   _collision_checker(nullptr, 1, nullptr),
   _smoother(nullptr),
   _costmap(nullptr),
+  _costmap_ros(nullptr),
   _costmap_downsampler(nullptr)
 {
 }
@@ -215,7 +216,7 @@ void SmacPlannerHybrid::configure(
   }
 
   // Initialize collision checker
-  _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
+  _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node);
   _collision_checker.setFootprint(
     _costmap_ros->getRobotFootprint(),
     _costmap_ros->getUseRadius(),
@@ -669,7 +670,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
 
     // Re-Initialize collision checker
     if (reinit_collision_checker) {
-      _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
+      _collision_checker = GridCollisionChecker(_costmap_ros, _angle_quantizations, node);
       _collision_checker.setFootprint(
         _costmap_ros->getRobotFootprint(),
         _costmap_ros->getUseRadius(),