Update nav2_system_tests to use cancel checker

Signed-off-by: Kemal Bektas <[email protected]>
ros-navigation · Feb 27, 2024 · 97c5701 · 97c5701
1 parent f71266d
commit 97c5701
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 14 deletions.
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
@@ -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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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<bool()>) 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
@@ -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
@@ -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"));