diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp index 99cf9e17770..f8581c37bc3 100644 --- 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 & slayer) { - nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer(); + slayer = std::make_shared(); layers.addPlugin(std::shared_ptr(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 & olayer) { - nav2_costmap_2d::ObstacleLayer * olayer = new nav2_costmap_2d::ObstacleLayer(); + olayer = std::make_shared(); olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/); layers.addPlugin(std::shared_ptr(olayer)); - return olayer; } void addObservation( - nav2_costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0, + std::shared_ptr & 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 & ilayer) { - nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer(); + ilayer = std::make_shared(); ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/); std::shared_ptr 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 index 82d84cce7b8..0337590a174 100644 --- 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 & ilayer, double inflation_radius); void initNode(double inflation_radius); - void waitForMap(nav2_costmap_2d::StaticLayer * slayer); + void waitForMap(std::shared_ptr & slayer); protected: nav2_util::LifecycleNode::SharedPtr node_; @@ -106,7 +106,7 @@ std::vector TestNode::setRadii( return polygon; } -void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer) +void TestNode::waitForMap(std::shared_ptr & 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 & 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 polygon = setRadii(layers, 2.1, 2.3); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 polygon = setRadii(layers, 2.1, 2.3); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 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 olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 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 olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 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 slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 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 slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 polygon = setRadii(layers, 1, 1.75); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr 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 index b499e65398d..7e1910f5f76 100644 --- 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 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 index ef58b069df2..ae73b914f64 100644 --- 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 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 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 index e9d62d13372..864ca4c3e3c 100644 --- 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(map_server_node_name_, node_); + std::make_shared("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 lifecycle_client_; + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr lifecycle_client_; }; + +rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr; +std::shared_ptr 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(); auto client = node_->create_client( - "/" + 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(); auto client = node_->create_client( - "/" + 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(); auto client = node_->create_client( - "/" + 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()); diff --git a/nav2_map_server/test/component/test_map_server_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp index ce31258ec0d..373f8783056 100644 --- a/nav2_map_server/test/component/test_map_server_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -38,25 +38,23 @@ class RclCppFixture RclCppFixture g_rclcppfixture; -class TestNode : public ::testing::Test +class MapServerTestFixture : public ::testing::Test { public: - TestNode() + static void SetUpTestCase() { node_ = rclcpp::Node::make_shared("map_client_test"); - map_server_node_name_ = "map_server"; lifecycle_client_ = - std::make_shared(map_server_node_name_, node_); + std::make_shared("map_server", 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); @@ -64,7 +62,6 @@ class TestNode : public ::testing::Test template typename T::Response::SharedPtr send_request( - rclcpp::Node::SharedPtr node, typename rclcpp::Client::SharedPtr client, typename T::Request::SharedPtr request) @@ -72,10 +69,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; @@ -95,18 +89,23 @@ class TestNode : public ::testing::Test } } - std::string map_server_node_name_; - rclcpp::Node::SharedPtr node_; - std::shared_ptr lifecycle_client_; + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr lifecycle_client_; }; + +rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr; +std::shared_ptr MapServerTestFixture::lifecycle_client_ = + nullptr; + + // Send map getting service request and verify obtained OccupancyGrid -TEST_F(TestNode, GetMap) +TEST_F(MapServerTestFixture, GetMap) { RCLCPP_INFO(node_->get_logger(), "Testing GetMap service"); auto req = std::make_shared(); auto client = node_->create_client( - "/" + map_server_node_name_ + "/map"); + "/map_server/map"); RCLCPP_INFO(node_->get_logger(), "Waiting for map service"); ASSERT_TRUE(client->wait_for_service()); @@ -117,12 +116,12 @@ TEST_F(TestNode, GetMap) } // Send map loading service request and verify obtained OccupancyGrid -TEST_F(TestNode, LoadMap) +TEST_F(MapServerTestFixture, LoadMap) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); auto client = node_->create_client( - "/" + map_server_node_name_ + "/load_map"); + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -135,12 +134,12 @@ TEST_F(TestNode, LoadMap) } // Send map loading service request without specifying which map to load -TEST_F(TestNode, LoadMapNull) +TEST_F(MapServerTestFixture, LoadMapNull) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); auto client = node_->create_client( - "/" + map_server_node_name_ + "/load_map"); + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -153,12 +152,12 @@ TEST_F(TestNode, LoadMapNull) } // Send map loading service request with non-existing yaml file -TEST_F(TestNode, LoadMapInvalidYaml) +TEST_F(MapServerTestFixture, LoadMapInvalidYaml) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); auto client = node_->create_client( - "/" + map_server_node_name_ + "/load_map"); + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -171,12 +170,12 @@ TEST_F(TestNode, LoadMapInvalidYaml) } // Send map loading service request with yaml file containing non-existing map -TEST_F(TestNode, LoadMapInvalidImage) +TEST_F(MapServerTestFixture, LoadMapInvalidImage) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); auto client = node_->create_client( - "/" + map_server_node_name_ + "/load_map"); + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp index c7a66482cb9..cb83ed981e5 100644 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" @@ -25,6 +27,14 @@ using namespace std::chrono_literals; using nav2_system_tests::SpinRecoveryTester; +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + class SpinRecoveryTestFixture : public ::testing::TestWithParam> { @@ -76,7 +86,8 @@ INSTANTIATE_TEST_CASE_P( std::make_tuple(M_PIf32, 0.1), std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), std::make_tuple(-2.0 * M_PIf32, 0.1), - std::make_tuple(4.0 * M_PIf32, 0.15)), ); + std::make_tuple(4.0 * M_PIf32, 0.15)), + testNameGenerator); int main(int argc, char ** argv) { diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp index 4ed4db839ab..e14ca5c7dcf 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include "rclcpp/rclcpp.hpp" @@ -25,6 +27,14 @@ using namespace std::chrono_literals; using nav2_system_tests::WaitRecoveryTester; +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + class WaitRecoveryTestFixture : public ::testing::TestWithParam> { @@ -71,7 +81,8 @@ INSTANTIATE_TEST_CASE_P( ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), - std::make_tuple(5.0, 0.0)), ); + std::make_tuple(5.0, 0.0)), + testNameGenerator); int main(int argc, char ** argv) {