From fb765f11c9bb15b8ce426b3b9e1c8df536339e95 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 15:14:42 -0800 Subject: [PATCH 01/12] debugging flaky shutdown on plugin planner tests --- nav2_planner/src/planner_server.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e72abb3223..e48422809c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -198,18 +198,26 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Cleaning up"); action_server_pose_.reset(); + RCLCPP_INFO(get_logger(), "Cleaning upA"); action_server_poses_.reset(); + RCLCPP_INFO(get_logger(), "Cleaning upB"); plan_publisher_.reset(); + RCLCPP_INFO(get_logger(), "Cleaning upC"); tf_.reset(); + RCLCPP_INFO(get_logger(), "Cleaning upD"); costmap_ros_->on_cleanup(state); + RCLCPP_INFO(get_logger(), "Cleaning upE"); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { + RCLCPP_INFO(get_logger(), "Cleaning upF"); it->second->cleanup(); } + RCLCPP_INFO(get_logger(), "Cleaning upG"); planners_.clear(); + RCLCPP_INFO(get_logger(), "Cleaning upH"); costmap_ = nullptr; - + RCLCPP_INFO(get_logger(), "Cleaning upI"); return nav2_util::CallbackReturn::SUCCESS; } From ab67968c4e0074550203f2a568ff68729446d508 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 15:21:27 -0800 Subject: [PATCH 02/12] adding independent tests --- .../src/planning/test_planner_plugins.cpp | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 5d4b52eb87..aab0e8c726 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -62,12 +62,30 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); obj->onCleanup(state); +} + +void testSmallPathValidityAndNoOrientation(std::string plugin, double length) +{ + auto obj = std::make_shared(); + rclcpp_lifecycle::State state; + obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin)); // Test WITH use_final_approach_orientation // expecting end path pose orientation to be equal to approach orientation // which in the one pose corner case should be the start pose orientation obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); obj->onConfigure(state); + + start.pose.position.x = 0.5; + start.pose.position.y = 0.5; + start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); + start.header.frame_id = "map"; + + goal.pose.position.x = 0.5; + goal.pose.position.y = start.pose.position.y + length; + goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); + goal.header.frame_id = "map"; + path = obj->getPlan(start, goal, "GridBased"); EXPECT_GT((int)path.poses.size(), 0); @@ -115,76 +133,151 @@ TEST(testPluginMap, Smac2dEqualStartGoal) testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); } +TEST(testPluginMap, Smac2dEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.0); +} + TEST(testPluginMap, Smac2dVerySmallPath) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001); } +TEST(testPluginMap, Smac2dVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.00001); +} + TEST(testPluginMap, Smac2dBelowCostmapResolution) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.09); } +TEST(testPluginMap, Smac2dBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.09); +} + TEST(testPluginMap, Smac2dJustAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.102); } +TEST(testPluginMap, Smac2dJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 0.102); +} + TEST(testPluginMap, Smac2dAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 1.5); } +TEST(testPluginMap, Smac2dAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_smac_planner/SmacPlanner2D", 1.5); +} + TEST(testPluginMap, NavFnEqualStartGoal) { testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.0); } +TEST(testPluginMap, NavFnEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.0); +} + TEST(testPluginMap, NavFnVerySmallPath) { testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001); } +TEST(testPluginMap, NavFnVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.00001); +} + TEST(testPluginMap, NavFnBelowCostmapResolution) { testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.09); } +TEST(testPluginMap, NavFnBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.09); +} + TEST(testPluginMap, NavFnJustAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 0.102); } +TEST(testPluginMap, NavFnJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 0.102); +} + TEST(testPluginMap, NavFnAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_navfn_planner/NavfnPlanner", 1.5); } +TEST(testPluginMap, NavFnAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_navfn_planner/NavfnPlanner", 1.5); +} + TEST(testPluginMap, ThetaStarEqualStartGoal) { testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0); } +TEST(testPluginMap, ThetaStarEqualStartGoalN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.0); +} + TEST(testPluginMap, ThetaStarVerySmallPath) { testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001); } +TEST(testPluginMap, ThetaStarVerySmallPathN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.00001); +} + TEST(testPluginMap, ThetaStarBelowCostmapResolution) { testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09); } +TEST(testPluginMap, ThetaStarBelowCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.09); +} + TEST(testPluginMap, ThetaStarJustAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102); } +TEST(testPluginMap, ThetaStarJustAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 0.102); +} + TEST(testPluginMap, ThetaStarAboveCostmapResolution) { testSmallPathValidityAndOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); } +TEST(testPluginMap, ThetaStarAboveCostmapResolutionN) +{ + testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From 29f63d1fbe9fb839e18b2b879b87e91ba8a6f1e5 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 15:28:56 -0800 Subject: [PATCH 03/12] adding types --- nav2_system_tests/src/planning/test_planner_plugins.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index aab0e8c726..be7280110a 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -76,6 +76,9 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); obj->onConfigure(state); + geometry_msgs::msg::PoseStamped start; + geometry_msgs::msg::PoseStamped goal; + start.pose.position.x = 0.5; start.pose.position.y = 0.5; start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2); @@ -86,7 +89,7 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; - path = obj->getPlan(start, goal, "GridBased"); + auto path = obj->getPlan(start, goal, "GridBased"); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); From 2157f1dfa2131e0e7ae3f416266066957cb963db Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 15:55:55 -0800 Subject: [PATCH 04/12] adding new QoS and fix param declares --- nav2_system_tests/src/planning/CMakeLists.txt | 1 + nav2_system_tests/src/planning/test_planner_plugins.cpp | 2 ++ .../src/recoveries/spin/spin_recovery_tester.cpp | 4 +++- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 8abf261192..e50b959184 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -48,6 +48,7 @@ ament_add_test(test_planner_random ament_add_gtest(test_planner_plugins test_planner_plugins.cpp + TIMEOUT 10 ) ament_target_dependencies(test_planner_plugins rclcpp geometry_msgs nav2_msgs ${dependencies}) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index be7280110a..a3ef502ec0 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -73,6 +73,8 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) // Test WITH use_final_approach_orientation // expecting end path pose orientation to be equal to approach orientation // which in the one pose corner case should be the start pose orientation + obj->declare_parameter( + "GridBased.use_final_approach_orientation", rclcpp::ParameterValue(true)); obj->set_parameter(rclcpp::Parameter("GridBased.use_final_approach_orientation", true)); obj->onConfigure(state); diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 5a0337b407..1ecb72fa18 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -57,7 +57,9 @@ SpinRecoveryTester::SpinRecoveryTester() publisher_ = node_->create_publisher("initialpose", 10); fake_costmap_publisher_ = - node_->create_publisher("local_costmap/costmap_raw", 10); + node_->create_publisher( + "local_costmap/costmap_raw", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), From 2c8b9a884d5447ec312ceb4ea1b4cfb989c0cddb Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 16:51:36 -0800 Subject: [PATCH 05/12] try this --- nav2_planner/src/planner_server.cpp | 9 --------- .../src/recoveries/spin/spin_recovery_tester.cpp | 6 +++--- .../src/recoveries/spin/spin_recovery_tester.hpp | 3 --- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e48422809c..fa6a9f4b8c 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -198,26 +198,17 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "Cleaning up"); action_server_pose_.reset(); - RCLCPP_INFO(get_logger(), "Cleaning upA"); action_server_poses_.reset(); - RCLCPP_INFO(get_logger(), "Cleaning upB"); plan_publisher_.reset(); - RCLCPP_INFO(get_logger(), "Cleaning upC"); tf_.reset(); - RCLCPP_INFO(get_logger(), "Cleaning upD"); costmap_ros_->on_cleanup(state); - RCLCPP_INFO(get_logger(), "Cleaning upE"); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { - RCLCPP_INFO(get_logger(), "Cleaning upF"); it->second->cleanup(); } - RCLCPP_INFO(get_logger(), "Cleaning upG"); planners_.clear(); - RCLCPP_INFO(get_logger(), "Cleaning upH"); costmap_ = nullptr; - RCLCPP_INFO(get_logger(), "Cleaning upI"); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 1ecb72fa18..270b5cb76d 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -58,8 +58,8 @@ SpinRecoveryTester::SpinRecoveryTester() node_->create_publisher("initialpose", 10); fake_costmap_publisher_ = node_->create_publisher( - "local_costmap/costmap_raw", - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + "local_costmap/costmap_raw", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -186,7 +186,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(1)); + rclcpp::sleep_for(std::chrono::milliseconds(5)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp index 4ffd429001..8c207dd4a8 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp @@ -92,9 +92,6 @@ class SpinRecoveryTester // Publisher to publish fake costmap raw rclcpp::Publisher::SharedPtr fake_costmap_publisher_; - // Publisher to publish fake costmap footprint - rclcpp::Publisher::SharedPtr fake_footprint_publisher_; - // Subscriber for amcl pose rclcpp::Subscription::SharedPtr subscription_; From f5c9389e9ea4fd48fd7acf439ebc80782429f8ee Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 17:22:45 -0800 Subject: [PATCH 06/12] try tihis --- nav2_system_tests/src/planning/test_planner_plugins.cpp | 6 ++++-- .../src/recoveries/spin/spin_recovery_tester.cpp | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index a3ef502ec0..a56a7216b1 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -61,7 +61,8 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) auto path = obj->getPlan(start, goal, "GridBased"); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); - obj->onCleanup(state); + // obj->onCleanup(state); + obj.reset(); } void testSmallPathValidityAndNoOrientation(std::string plugin, double length) @@ -108,7 +109,8 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) atan2(dy, dx), 0.01); } - obj->onCleanup(state); + // obj->onCleanup(state); + obj.reset(); } TEST(testPluginMap, Failures) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 270b5cb76d..587289a666 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -186,7 +186,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(5)); + rclcpp::sleep_for(std::chrono::milliseconds(10)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); From 46a86a1c33c7a6cb61dd115dc68c0295856df92f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 18:50:02 -0800 Subject: [PATCH 07/12] try 10hz --- nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 587289a666..5dd15d2f8b 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -186,7 +186,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(10)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); From 4c02cfbf9dd31a73816ab7f07a4d8ce81c7e325e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 18:53:17 -0800 Subject: [PATCH 08/12] try sleeping on setup --- .../src/recoveries/spin/test_spin_recovery_node.cpp | 1 + 1 file changed, 1 insertion(+) 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 059331eaae..0b75ec206d 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 @@ -44,6 +44,7 @@ class SpinRecoveryTestFixture if (!spin_recovery_tester->isActive()) { spin_recovery_tester->activate(); } + std::this_thread::sleep_for(2000ms); } static void TearDownTestCase() From d00bbdefb3a3e146792232ac3370f13d103f7748 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 19:32:45 -0800 Subject: [PATCH 09/12] add footprint pub --- .../recoveries/spin/spin_recovery_tester.cpp | 18 +++++++++++++++++- .../recoveries/spin/spin_recovery_tester.hpp | 3 +++ .../spin/test_spin_recovery_node.cpp | 1 - 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 5dd15d2f8b..b05f05d0c8 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -60,6 +60,8 @@ SpinRecoveryTester::SpinRecoveryTester() node_->create_publisher( "local_costmap/costmap_raw", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + fake_footprint_publisher_ = node_->create_publisher( + "local_costmap/published_footprint", rclcpp::SystemDefaultsQoS()); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -186,7 +188,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( { sendFakeOdom(command_yaw); sendFakeCostmap(target_yaw); - rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::sleep_for(std::chrono::milliseconds(1)); } sendFakeOdom(target_yaw); sendFakeCostmap(target_yaw); @@ -314,6 +316,20 @@ void SpinRecoveryTester::sendFakeOdom(float angle) transformStamped.transform.rotation.w = q.w(); tf_broadcaster_->sendTransform(transformStamped); + + geometry_msgs::msg::PolygonStamped footprint; + footprint.header.frame_id = "odom"; + footprint.header.stamp = rclcpp::Time(); + footprint.polygon.points.resize(4); + footprint.polygon.points[0].x = 0.22; + footprint.polygon.points[0].y = 0.22; + footprint.polygon.points[1].x = 0.22; + footprint.polygon.points[1].y = -0.22; + footprint.polygon.points[2].x = -0.22; + footprint.polygon.points[2].y = -0.22; + footprint.polygon.points[3].x = -0.22; + footprint.polygon.points[3].y = 0.22; + fake_footprint_publisher_->publish(footprint); } void SpinRecoveryTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp index 8c207dd4a8..4ffd429001 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp @@ -92,6 +92,9 @@ class SpinRecoveryTester // Publisher to publish fake costmap raw rclcpp::Publisher::SharedPtr fake_costmap_publisher_; + // Publisher to publish fake costmap footprint + rclcpp::Publisher::SharedPtr fake_footprint_publisher_; + // Subscriber for amcl pose rclcpp::Subscription::SharedPtr subscription_; 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 0b75ec206d..059331eaae 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 @@ -44,7 +44,6 @@ class SpinRecoveryTestFixture if (!spin_recovery_tester->isActive()) { spin_recovery_tester->activate(); } - std::this_thread::sleep_for(2000ms); } static void TearDownTestCase() From ff1e3ddd854661a1f1eae498e92cea4c0808820d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 20:26:03 -0800 Subject: [PATCH 10/12] adding additional pub for debug --- .../src/recoveries/spin/spin_recovery_tester.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index b05f05d0c8..301455af00 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -155,6 +155,13 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( "Init Yaw is %lf", fabs(tf2::getYaw(initial_pose.pose.orientation))); RCLCPP_INFO(node_->get_logger(), "Before sending goal"); + + // Intialize fake costmap + if (make_fake_costmap_) { + sendFakeCostmap(target_yaw); + sendFakeOdom(0.0); + } + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != From bb41a019dfe8a7000c78f0c4a38d1df25024212e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 20:58:17 -0800 Subject: [PATCH 11/12] try adding a spin for debug types --- nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 301455af00..884fb7a9b5 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -162,6 +162,8 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( sendFakeOdom(0.0); } + rclcpp::spin_some(node_); + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != From b1af8203e96c03f015af0e0cef1b6433e7c30bba Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 17 Nov 2021 22:19:16 -0800 Subject: [PATCH 12/12] replacing spin with sleep --- .../src/recoveries/spin/spin_recovery_tester.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp index 884fb7a9b5..73a66bd755 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -162,7 +162,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( sendFakeOdom(0.0); } - rclcpp::spin_some(node_); + rclcpp::sleep_for(std::chrono::milliseconds(100)); auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); @@ -328,7 +328,7 @@ void SpinRecoveryTester::sendFakeOdom(float angle) geometry_msgs::msg::PolygonStamped footprint; footprint.header.frame_id = "odom"; - footprint.header.stamp = rclcpp::Time(); + footprint.header.stamp = node_->now(); footprint.polygon.points.resize(4); footprint.polygon.points[0].x = 0.22; footprint.polygon.points[0].y = 0.22;