diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp index 2b6ae7971d3..89393ab8a5b 100644 --- a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -51,7 +51,8 @@ class UnknownErrorPlanner : public nav2_core::GlobalPlanner nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerException("Unknown Error"); } @@ -61,7 +62,8 @@ class StartOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOccupied("Start Occupied"); } @@ -71,7 +73,8 @@ class GoalOccupiedErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOccupied("Goal occupied"); } @@ -81,7 +84,8 @@ class StartOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); } @@ -91,7 +95,8 @@ class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); } @@ -101,7 +106,8 @@ class NoValidPathErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { return nav_msgs::msg::Path(); } @@ -112,7 +118,8 @@ class TimedOutErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTimedOut("Planner Timed Out"); } @@ -122,7 +129,8 @@ class TFErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::PlannerTFError("TF Error"); } @@ -132,7 +140,8 @@ class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner { nav_msgs::msg::Path createPlan( const geometry_msgs::msg::PoseStamped &, - const geometry_msgs::msg::PoseStamped &) override + const geometry_msgs::msg::PoseStamped &, + std::function) override { throw nav2_core::NoViapointsGiven("No Via points given"); } diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index 7a5f8d4b7f9..421a00f4b7d 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -87,7 +87,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer return false; } try { - path = planners_["GridBased"]->createPlan(start, goal); + auto dummy_cancel_checker = []() {return false;}; + path = planners_["GridBased"]->createPlan(start, goal, dummy_cancel_checker); // The situation when createPlan() did not throw any exception // does not guarantee that plan was created correctly. // So it should be checked additionally that path is correct. diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 906e8f44d3a..cb60d6350ff 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -57,9 +57,11 @@ void testSmallPathValidityAndOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; + auto dummy_cancel_checker = []() {return false;}; + // Test without use_final_approach_orientation // expecting end path pose orientation to be equal to goal orientation - auto path = obj->getPlan(start, goal, "GridBased"); + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); EXPECT_NEAR(tf2::getYaw(path.poses.back().pose.orientation), -M_PI, 0.01); // obj->onCleanup(state); @@ -93,7 +95,9 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length) goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI); goal.header.frame_id = "map"; - auto path = obj->getPlan(start, goal, "GridBased"); + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, "GridBased", dummy_cancel_checker); EXPECT_GT((int)path.poses.size(), 0); int path_size = path.poses.size(); @@ -127,11 +131,14 @@ TEST(testPluginMap, Failures) geometry_msgs::msg::PoseStamped goal; std::string plugin_fake = "fake"; std::string plugin_none = ""; - auto path = obj->getPlan(start, goal, plugin_none); + + auto dummy_cancel_checker = []() {return false;}; + + auto path = obj->getPlan(start, goal, plugin_none, dummy_cancel_checker); EXPECT_EQ(path.header.frame_id, std::string("map")); try { - path = obj->getPlan(start, goal, plugin_fake); + path = obj->getPlan(start, goal, plugin_fake, dummy_cancel_checker); FAIL() << "Failed to throw invalid planner id exception"; } catch (const nav2_core::InvalidPlanner & ex) { EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid"));