From 7d5e93c5704647055ef8a14636373b4f997de76b Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 7 Jun 2023 16:04:48 -0700 Subject: [PATCH 1/3] CI test fixing for BT.CPP (paired with BehaviorTree.CPP/579) --- .../decorator/test_single_trigger_node.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp index c7d2489b4c6..a3bd70afd56 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp @@ -24,13 +24,24 @@ using namespace std::chrono; // NOLINT using namespace std::chrono_literals; // NOLINT +// Shim BT node to access protected resetStatus method +class ShimNode : public nav2_behavior_tree::SingleTrigger +{ +public: + ShimNode(const std::string & name, + const BT::NodeConfiguration & confi) + : SingleTrigger(name, confi) + {} + ~ShimNode() {} + void changeStatus() {resetStatus();} +}; + class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture { public: void SetUp() { - bt_node_ = std::make_shared( - "single_trigger", *config_); + bt_node_ = std::make_shared("single_trigger", *config_); dummy_node_ = std::make_shared(); bt_node_->setChild(dummy_node_.get()); } @@ -42,11 +53,11 @@ class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixt } protected: - static std::shared_ptr bt_node_; + static std::shared_ptr bt_node_; static std::shared_ptr dummy_node_; }; -std::shared_ptr +std::shared_ptr SingleTriggerTestFixture::bt_node_ = nullptr; std::shared_ptr SingleTriggerTestFixture::dummy_node_ = nullptr; @@ -71,6 +82,7 @@ TEST_F(SingleTriggerTestFixture, test_behavior) // halt BT for a new execution run, should work when dummy node is running // and once when dummy node returns success and then fail bt_node_->halt(); + bt_node_->changeStatus(); // BTv3.8+ doesn't reset root node automatically dummy_node_->changeStatus(BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); From bb435e263f997c514958dbb7bd6994b10dd9fd76 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 7 Jun 2023 16:07:36 -0700 Subject: [PATCH 2/3] lint --- .../test/plugins/decorator/test_single_trigger_node.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp index a3bd70afd56..0d7dc12b2f5 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_single_trigger_node.cpp @@ -28,8 +28,9 @@ using namespace std::chrono_literals; // NOLINT class ShimNode : public nav2_behavior_tree::SingleTrigger { public: - ShimNode(const std::string & name, - const BT::NodeConfiguration & confi) + ShimNode( + const std::string & name, + const BT::NodeConfiguration & confi) : SingleTrigger(name, confi) {} ~ShimNode() {} From 9a4f9a72af823fe9098f30eba8c14d8b1b44ee06 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Thu, 8 Jun 2023 09:58:50 -0700 Subject: [PATCH 3/3] fixing smoother unit test failures --- nav2_smoother/test/test_smoother_server.cpp | 38 ++++++++++----------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/nav2_smoother/test/test_smoother_server.cpp b/nav2_smoother/test/test_smoother_server.cpp index d7afe9b55d9..a84b1cdc323 100644 --- a/nav2_smoother/test/test_smoother_server.cpp +++ b/nav2_smoother/test/test_smoother_server.cpp @@ -223,21 +223,16 @@ class DummySmootherServer : public nav2_smoother::SmootherServer }; // Define a test class to hold the context for the tests - -class SmootherConfigTest : public ::testing::Test -{ -}; - class SmootherTest : public ::testing::Test { -protected: - SmootherTest() {SetUp();} +public: + SmootherTest() {} ~SmootherTest() {} - void SetUp() + void SetUp() override { - node_lifecycle_ = - std::make_shared( + node_ = + std::make_shared( "LifecycleSmootherTestNode", rclcpp::NodeOptions()); smoother_server_ = std::make_shared(); @@ -252,10 +247,10 @@ class SmootherTest : public ::testing::Test smoother_server_->activate(); client_ = rclcpp_action::create_client( - node_lifecycle_->get_node_base_interface(), - node_lifecycle_->get_node_graph_interface(), - node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "smooth_path"); + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), "smooth_path"); std::cout << "Setup complete." << std::endl; } @@ -264,6 +259,9 @@ class SmootherTest : public ::testing::Test smoother_server_->deactivate(); smoother_server_->cleanup(); smoother_server_->shutdown(); + smoother_server_.reset(); + client_.reset(); + node_.reset(); } bool sendGoal( @@ -291,7 +289,7 @@ class SmootherTest : public ::testing::Test auto future_goal = client_->async_send_goal(goal); - if (rclcpp::spin_until_future_complete(node_lifecycle_, future_goal) != + if (rclcpp::spin_until_future_complete(node_, future_goal) != rclcpp::FutureReturnCode::SUCCESS) { std::cout << "failed sending goal" << std::endl; @@ -315,12 +313,12 @@ class SmootherTest : public ::testing::Test std::cout << "Getting async result..." << std::endl; auto future_result = client_->async_get_result(goal_handle_); std::cout << "Waiting on future..." << std::endl; - rclcpp::spin_until_future_complete(node_lifecycle_, future_result); + rclcpp::spin_until_future_complete(node_, future_result); std::cout << "future received!" << std::endl; return future_result.get(); } - std::shared_ptr node_lifecycle_; + std::shared_ptr node_; std::shared_ptr smoother_server_; std::shared_ptr> client_; std::shared_ptr> goal_handle_; @@ -387,7 +385,7 @@ TEST_F(SmootherTest, testingCollisionCheckDisabled) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -402,7 +400,7 @@ TEST_F(SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) +TEST(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) { auto smoother_server = std::make_shared(); smoother_server->set_parameter( @@ -417,7 +415,7 @@ TEST_F(SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin) SUCCEED(); } -TEST_F(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) +TEST(SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin) { auto smoother_server = std::make_shared(); auto state = smoother_server->configure();