complete unit test conversions

nav2_smac_planner/test/test_node2d.cpp

@@ -36,8 +36,15 @@ TEST(Node2DTest, test_node_2d)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 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, node);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, 72, node);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // test construction
@@ -123,8 +130,15 @@ TEST(Node2DTest, test_node_2d_neighbors)
   EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101);
 
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 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);
   unsigned char cost = static_cast<unsigned int>(1);
   nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1);
   node->setCost(cost);

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -57,8 +57,15 @@ TEST(NodeHybridTest, test_node_hybrid)
 
   nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
     10, 10, 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, node);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, 72, node);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // test construction
@@ -200,10 +207,10 @@ TEST(NodeHybridTest, test_obstacle_heuristic)
 
   // first block the high-cost passage to make sure the cost spreads through the better path
   for (unsigned int j = 61; j <= 70; ++j) {
-    costmapA->setCost(50, j, 254);
+    costmap->setCost(50, j, 254);
   }
   nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
-    costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
+    costmap_ros, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
   float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
     testA.pose,
     testB.pose,
@@ -215,10 +222,10 @@ TEST(NodeHybridTest, test_obstacle_heuristic)
   // (it should, since the unblocked narrow path will have higher cost than the wide one
   //  and thus lower bound of the path cost should be unchanged)
   for (unsigned int j = 61; j <= 70; ++j) {
-    costmapA->setCost(50, j, 250);
+    costmap->setCost(50, j, 250);
   }
   nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
-    costmapA,
+    costmap_ros,
     testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y);
   float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
     testA.pose,