ros-navigation · SteveMacenski · May 11, 2020 · May 11, 2020 · May 11, 2020 · May 11, 2020
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp
@@ -77,28 +77,28 @@ unsigned int countValues(
   return count;
 }
 
-nav2_costmap_2d::StaticLayer * addStaticLayer(
+void addStaticLayer(
   nav2_costmap_2d::LayeredCostmap & layers,
-  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
 {
-  nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer();
+  slayer = std::make_shared<nav2_costmap_2d::StaticLayer>();
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/);
-  return slayer;
 }
 
-nav2_costmap_2d::ObstacleLayer * addObstacleLayer(
+void addObstacleLayer(
   nav2_costmap_2d::LayeredCostmap & layers,
-  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer)
 {
-  nav2_costmap_2d::ObstacleLayer * olayer = new nav2_costmap_2d::ObstacleLayer();
+  olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>();
   olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/);
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(olayer));
-  return olayer;
 }
 
 void addObservation(
-  nav2_costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0,
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer, double x, double y, double z = 0.0,
   double ox = 0.0, double oy = 0.0, double oz = MAX_Z)
 {
   sensor_msgs::msg::PointCloud2 cloud;
@@ -122,15 +122,15 @@ void addObservation(
   olayer->addStaticObservation(obs, true, true);
 }
 
-nav2_costmap_2d::InflationLayer * addInflationLayer(
+void addInflationLayer(
   nav2_costmap_2d::LayeredCostmap & layers,
-  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
+  tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer)
 {
-  nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer();
+  ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>();
   ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/);
   std::shared_ptr<nav2_costmap_2d::Layer> ipointer(ilayer);
   layers.addPlugin(ipointer);
-  return ilayer;
 }
 
 

diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp
@@ -72,12 +72,12 @@ class TestNode : public ::testing::Test
   void validatePointInflation(
     unsigned int mx, unsigned int my,
     nav2_costmap_2d::Costmap2D * costmap,
-    nav2_costmap_2d::InflationLayer * ilayer,
+    std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
     double inflation_radius);
 
   void initNode(double inflation_radius);
 
-  void waitForMap(nav2_costmap_2d::StaticLayer * slayer);
+  void waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer);
 
 protected:
   nav2_util::LifecycleNode::SharedPtr node_;
@@ -106,7 +106,7 @@ std::vector<Point> TestNode::setRadii(
   return polygon;
 }
 
-void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer)
+void TestNode::waitForMap(std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer)
 {
   while (!slayer->isCurrent()) {
     rclcpp::spin_some(node_->get_node_base_interface());
@@ -117,7 +117,7 @@ void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer)
 void TestNode::validatePointInflation(
   unsigned int mx, unsigned int my,
   nav2_costmap_2d::Costmap2D * costmap,
-  nav2_costmap_2d::InflationLayer * ilayer,
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
   double inflation_radius)
 {
   bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
@@ -206,8 +206,12 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove)
   //               circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   addObservation(olayer, 0, 0, MAX_Z);
@@ -234,8 +238,12 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns)
   // circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   addObservation(olayer, 0, 0, MAX_Z);
@@ -260,8 +268,12 @@ TEST_F(TestNode, testCostFunctionCorrectness)
   //               circumscribed radius = 8.0
   std::vector<Point> polygon = setRadii(layers, 5.0, 6.25);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   addObservation(olayer, 50, 50, MAX_Z);
@@ -331,8 +343,12 @@ TEST_F(TestNode, testInflationOrderCorrectness)
   //               circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   // Add two diagonal cells, they would induce problems under the
@@ -359,9 +375,14 @@ TEST_F(TestNode, testInflation)
   // circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 1, 1);
 
-  auto slayer = addStaticLayer(layers, tf, node_);
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
   layers.setFootprint(polygon);
 
   nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
@@ -432,9 +453,15 @@ TEST_F(TestNode, testInflation2)
   // circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 1, 1);
 
-  auto slayer = addStaticLayer(layers, tf, node_);
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
+
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   waitForMap(slayer);
@@ -464,8 +491,12 @@ TEST_F(TestNode, testInflation3)
   // 1 2 3
   std::vector<Point> polygon = setRadii(layers, 1, 1.75);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
-  addInflationLayer(layers, tf, node_);
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
+  addObstacleLayer(layers, tf, node_, olayer);
+
+  std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+  addInflationLayer(layers, tf, node_, ilayer);
+
   layers.setFootprint(polygon);
 
   // There should be no occupied cells

diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp
@@ -157,7 +157,7 @@ TEST_F(TestNode, testRaytracing) {
 
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf, node_);
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // Add a point at 0, 0, 0
   addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2);
@@ -179,7 +179,7 @@ TEST_F(TestNode, testRaytracing2) {
   tf2_ros::Buffer tf(node_->get_clock());
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf, node_);
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // If we print map now, it is 10x10 all value 0
   // printMap(*(layers.getCostmap()));
@@ -236,7 +236,7 @@ TEST_F(TestNode, testWaveInterference) {
   // Start with an empty map, no rolling window, tracking unknown
   nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
   layers.resizeMap(10, 10, 1, 0, 0);
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
   // printMap(*(layers.getCostmap()));
@@ -265,7 +265,7 @@ TEST_F(TestNode, testZThreshold) {
   nav2_costmap_2d::LayeredCostmap layers("frame", false, true);
   layers.resizeMap(10, 10, 1, 0, 0);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // A point cloud with 2 points falling in a cell with a non-lethal cost
   addObservation(olayer, 0.0, 5.0, 0.4);
@@ -285,7 +285,7 @@ TEST_F(TestNode, testDynamicObstacles) {
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf, node_);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // Add a point cloud and verify its insertion. There should be only one new one
   addObservation(olayer, 0.0, 0.0);
@@ -310,7 +310,7 @@ TEST_F(TestNode, testMultipleAdditions) {
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
   addStaticLayer(layers, tf, node_);
 
-  nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
+  auto olayer = addObstacleLayer(layers, tf, node_);
 
   // A point cloud with one point that falls within an existing obstacle
   addObservation(olayer, 9.5, 0.0);
@@ -327,7 +327,9 @@ TEST_F(TestNode, testMultipleAdditions) {
 TEST_F(TestNode, testRepeatedResets) {
   tf2_ros::Buffer tf(node_->get_clock());
   nav2_costmap_2d::LayeredCostmap layers("frame", false, false);
-  addStaticLayer(layers, tf, node_);
+
+  std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+  addStaticLayer(layers, tf, node_, slayer);
 
   // TODO(orduno) Add obstacle layer
 

diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
@@ -128,12 +128,15 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false);
     // Add Static Layer
-    auto slayer = addStaticLayer(*layers_, *tf_buffer_, shared_from_this());
+    std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
+    addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer);
+
     while (!slayer->isCurrent()) {
       rclcpp::spin_some(this->get_node_base_interface());
     }
     // Add Inflation Layer
-    addInflationLayer(*layers_, *tf_buffer_, shared_from_this());
+    std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
+    addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer);
 
     return nav2_util::CallbackReturn::SUCCESS;
   }

diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp
@@ -39,25 +39,23 @@ class RclCppFixture
 
 RclCppFixture g_rclcppfixture;
 
-class TestNode : public ::testing::Test
+class MapSaverTestFixture : public ::testing::Test
 {
 public:
-  TestNode()
+  static void SetUpTestCase()
   {
     node_ = rclcpp::Node::make_shared("map_client_test");
-    map_server_node_name_ = "map_saver";
     lifecycle_client_ =
-      std::make_shared<nav2_util::LifecycleServiceClient>(map_server_node_name_, node_);
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_saver", node_);
     RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
 
-
-    std::this_thread::sleep_for(std::chrono::seconds(1));  // allow node to start up
+    std::this_thread::sleep_for(std::chrono::seconds(5));  // allow node to start up
     const std::chrono::seconds timeout(5);
     lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
     lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
   }
 
-  ~TestNode()
+  static void TearDownTestCase()
   {
     lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
     lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
@@ -73,10 +71,7 @@ class TestNode : public ::testing::Test
     auto result = client->async_send_request(request);
 
     // Wait for the result
-    if (
-      rclcpp::spin_until_future_complete(node, result) ==
-      rclcpp::executor::FutureReturnCode::SUCCESS)
-    {
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
       return result.get();
     } else {
       return nullptr;
@@ -96,19 +91,23 @@ class TestNode : public ::testing::Test
     }
   }
 
-  std::string map_server_node_name_;
-  rclcpp::Node::SharedPtr node_;
-  std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
 };
 
+
+rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
+  nullptr;
+
 // Send map saving service request.
 // Load saved map and verify obtained OccupancyGrid.
-TEST_F(TestNode, SaveMap)
+TEST_F(MapSaverTestFixture, SaveMap)
 {
   RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
   auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
   auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
-    "/" + map_server_node_name_ + "/save_map");
+    "/map_saver/save_map");
 
   RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
   ASSERT_TRUE(client->wait_for_service());
@@ -132,12 +131,12 @@ TEST_F(TestNode, SaveMap)
 
 // Send map saving service request with default parameters.
 // Load saved map and verify obtained OccupancyGrid.
-TEST_F(TestNode, SaveMapDefaultParameters)
+TEST_F(MapSaverTestFixture, SaveMapDefaultParameters)
 {
   RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
   auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
   auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
-    "/" + map_server_node_name_ + "/save_map");
+    "/map_saver/save_map");
 
   RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
   ASSERT_TRUE(client->wait_for_service());
@@ -162,12 +161,12 @@ TEST_F(TestNode, SaveMapDefaultParameters)
 // Send map saving service requests with different sets of parameters.
 // In case of map is expected to be saved correctly, load map from a saved
 // file and verify obtained OccupancyGrid.
-TEST_F(TestNode, SaveMapInvalidParameters)
+TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
 {
   RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
   auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
   auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
-    "/" + map_server_node_name_ + "/save_map");
+    "/map_saver/save_map");
 
   RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
   ASSERT_TRUE(client->wait_for_service());