[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

Author: SteveMacenski

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;
  }