Working final prototype to be tested

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -45,6 +45,7 @@
 #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 +162,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/CMakeLists.txt

@@ -155,7 +155,7 @@ if(BUILD_TESTING)
   set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h)
   ament_lint_auto_find_test_dependencies()
   find_package(ament_cmake_gtest REQUIRED)
-  # add_subdirectory(test)
+  add_subdirectory(test)
 endif()
 
 ament_export_include_directories(include ${OMPL_INCLUDE_DIRS})

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -11,7 +11,10 @@
 // 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"

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -431,6 +431,14 @@ 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
@@ -463,6 +471,7 @@ class NodeHybrid
 
   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/src/analytic_expansion.cpp

@@ -59,7 +59,7 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
     // See if we are closer and should be expanding more often
     const Coordinates node_coords =
       NodeT::getCoords(
-        current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size);
+      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)));

nav2_smac_planner/src/collision_checker.cpp

@@ -27,7 +27,7 @@ GridCollisionChecker::GridCollisionChecker(
     clock_ = node->get_clock();
     logger_ = node->get_logger();
   }
-  
+
   if (costmap_ros) {
     costmap_ros_ = costmap_ros;
   }

nav2_smac_planner/src/node_hybrid.cpp

@@ -42,6 +42,7 @@ 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;
@@ -487,6 +488,7 @@ 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);
   sampled_costmap = costmap_ros->getCostmap();
   if (motion_table.downsample_obstacle_heuristic) {
     std::weak_ptr<nav2_util::LifecycleNode> ptr;
@@ -531,40 +533,26 @@ void NodeHybrid::resetObstacleHeuristic(
   obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
 }
 
-inline float adjustedFootprintCost(
-  const float & cost, std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+float NodeHybrid::adjustedFootprintCost(const float & cost)
 {
-  // TODO efficiency: have radius check to know if needed at H cost start, find + store inflation layer
-  // TODO architecture: abstract out object in utils to share with MPPI
-  // TODO reenable and fix tests
-  if (costmap_ros->getUseRadius()) {
+  if (!inflation_layer) {
     return cost;
   }
 
   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) {
-      continue;
-    }
-
-    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;
+  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;
 
-    // Compute cost at this value
-    return static_cast<float>(
-      inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution()));
+  // Subtract minimum radius for edge cost
+  dist_to_obj -= min_radius;
+  if (dist_to_obj < 0.0f) {
+    dist_to_obj = 0.0f;
   }
 
-  // Didn't find an inflation layer; returning normal cost
-  return cost;
+  // Compute cost at this value
+  return static_cast<float>(
+    inflation_layer->computeCost(dist_to_obj / layered_costmap->getCostmap()->getResolution()));
 }
 
 
@@ -575,6 +563,7 @@ float NodeHybrid::getObstacleHeuristic(
 {
   // 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;
@@ -645,10 +634,13 @@ float NodeHybrid::getObstacleHeuristic(
       if (new_idx < size_x * size_y) {
         cost = static_cast<float>(sampled_costmap->getCost(new_idx));
 
-        // Adjust cost value if using SE2 footprint checks
-        cost = adjustedFootprintCost(cost, costmap_ros);
-
-        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/test/test_a_star.cpp

@@ -61,9 +61,15 @@ TEST(AStarTest, test_a_star_2d)
     }
   }
 
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
   // functional case testing
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 1, lnode);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, 1, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
   a_star.setCollisionChecker(checker.get());
   a_star.setStart(20u, 20u, 0);
@@ -149,8 +155,14 @@ TEST(AStarTest, test_a_star_se2)
     }
   }
 
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing
@@ -217,8 +229,14 @@ TEST(AStarTest, test_a_star_lattice)
     }
   }
 
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing
@@ -268,8 +286,14 @@ TEST(AStarTest, test_se2_single_pose_path)
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
 
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing

nav2_smac_planner/test/test_collision_checker.cpp

@@ -51,7 +51,13 @@ TEST(collision_footprint, test_basic)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmap_;
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
   collision_checker.inCollision(5.0, 5.0, 0.0, false);
   float cost = collision_checker.getCost();
@@ -64,7 +70,13 @@ TEST(collision_footprint, test_point_cost)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testB");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmap_;
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0);
 
@@ -79,7 +91,13 @@ TEST(collision_footprint, test_world_to_map)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testC");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmap_;
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0);
 
@@ -92,7 +110,7 @@ TEST(collision_footprint, test_world_to_map)
 
   EXPECT_NEAR(cost, 0.0, 0.001);
 
-  costmap_->setCost(50, 50, 200);
+  costmap->setCost(50, 50, 200);
   collision_checker.worldToMap(5.0, 5.0, x, y);
 
   collision_checker.inCollision(x, y, 0.0, false);
@@ -126,7 +144,13 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmap_;
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
   collision_checker.inCollision(50, 50, 0.0, false);
@@ -167,7 +191,13 @@ TEST(collision_footprint, test_point_and_line_cost)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmap_;
+
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_ros, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
   collision_checker.inCollision(50, 50, 0.0, false);

nav2_smac_planner/test/test_node2d.cpp

@@ -69,7 +69,7 @@ TEST(Node2DTest, test_node_2d)
   // check heuristic cost computation
   nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0);
   nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0);
-  EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02);
+  EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02);
 
   // check operator== works on index
   unsigned char costC = '2';

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -177,8 +177,15 @@ TEST(NodeHybridTest, test_obstacle_heuristic)
       costmapA->setCost(i, j, 254);
     }
   }
+
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 72, node);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, 72, node);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   nav2_smac_planner::NodeHybrid testA(0);
@@ -335,8 +342,15 @@ TEST(NodeHybridTest, test_node_reeds_neighbors)
   EXPECT_NEAR(nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01);
 
   nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0);
+
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = costmapA;
+
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, 72, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
   nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49);
   std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> neighborGetter =