Add system tests for cancel

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

Author: Kemal Bektas

nav2_system_tests/src/error_codes/error_code_param.yaml

@@ -154,7 +154,7 @@ planner_server:
     expected_planner_frequency: 20.0
     planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map",
                        "start_occupied", "goal_occupied", "timeout","no_valid_path",
-                       "no_viapoints_given" ]
+                       "no_viapoints_given", "cancelled" ]
     unknown:
       plugin: "nav2_system_tests::UnknownErrorPlanner"
     tf_error:
@@ -173,6 +173,8 @@ planner_server:
       plugin: "nav2_system_tests::NoValidPathErrorPlanner"
     no_viapoints_given:
       plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner"
+    cancelled:
+      plugin: "nav2_system_tests::CancelledPlanner"
 
 smoother_server:
   ros__parameters:

nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp

@@ -24,3 +24,4 @@ PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::Gl
 PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner)
 PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner)
 PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner)
+PLUGINLIB_EXPORT_CLASS(nav2_system_tests::CancelledPlanner, nav2_core::GlobalPlanner)

nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp

@@ -147,6 +147,26 @@ class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner
   }
 };
 
+class CancelledPlanner : public UnknownErrorPlanner
+{
+  nav_msgs::msg::Path createPlan(
+    const geometry_msgs::msg::PoseStamped &,
+    const geometry_msgs::msg::PoseStamped &,
+    std::function<bool()> cancel_checker) override
+  {
+    auto start_time = std::chrono::steady_clock::now();
+    while (rclcpp::ok() &&
+      std::chrono::steady_clock::now() - start_time < std::chrono::seconds(5))
+    {
+      if (cancel_checker()) {
+        throw nav2_core::PlannerCancelled("Planner Cancelled");
+      }
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+    }
+    throw nav2_core::PlannerException("Cancel is not called in time.");
+  }
+};
+
 }  // namespace nav2_system_tests
 
 

nav2_system_tests/src/error_codes/planner_plugins.xml

@@ -39,4 +39,8 @@
            base_class_type="nav2_core::GlobalPlanner">
         <description>This global planner produces a no via points exception.</description>
     </class>
+    <class type="nav2_system_tests::CancelledPlanner"
+           base_class_type="nav2_core::GlobalPlanner">
+        <description>This global planner produces a cancelled exception.</description>
+    </class>
 </library>

nav2_system_tests/src/error_codes/test_error_codes.py

@@ -14,6 +14,7 @@
 # limitations under the License.
 
 import sys
+import threading
 import time
 
 from geometry_msgs.msg import PoseStamped
@@ -23,7 +24,7 @@
     FollowPath,
     SmoothPath,
 )
-from nav2_simple_commander.robot_navigator import BasicNavigator
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
 from nav_msgs.msg import Path
 import rclpy
 
@@ -111,6 +112,17 @@ def main(argv=sys.argv[1:]):
             result.error_code == error_code
         ), 'Compute path to pose error does not match'
 
+    def cancel_task():
+        time.sleep(1)
+        navigator.goal_handle.cancel_goal_async()
+
+    # Check compute path to pose cancel
+    threading.Thread(target=cancel_task).start()
+    result = navigator._getPathImpl(initial_pose, goal_pose, 'cancelled')
+    assert (
+        navigator.getResult() == TaskResult.CANCELED
+    ), 'Compute path to pose cancel failed'
+
     # Check compute path through error codes
     goal_pose1 = goal_pose
     goal_poses = [goal_pose, goal_pose1]
@@ -133,6 +145,12 @@ def main(argv=sys.argv[1:]):
         assert (
             result.error_code == error_code
         ), 'Compute path through pose error does not match'
+    # Check compute path to pose cancel
+    threading.Thread(target=cancel_task).start()
+    result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, 'cancelled')
+    assert (
+        navigator.getResult() == TaskResult.CANCELED
+    ), 'Compute path through poses cancel failed'
 
     # Check compute path to pose error codes
     pose = PoseStamped()

nav2_system_tests/src/planning/test_planner_plugins.cpp

@@ -118,6 +118,36 @@ void testSmallPathValidityAndNoOrientation(std::string plugin, double length)
   obj.reset();
 }
 
+void testCancel(std::string plugin)
+{
+  auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
+  rclcpp_lifecycle::State state;
+  obj->set_parameter(rclcpp::Parameter("GridBased.plugin", plugin));
+  obj->declare_parameter("GridBased.terminal_checking_interval", rclcpp::ParameterValue(1));
+  obj->onConfigure(state);
+
+  geometry_msgs::msg::PoseStamped start;
+  geometry_msgs::msg::PoseStamped goal;
+
+  start.pose.position.x = 0.0;
+  start.pose.position.y = 0.0;
+  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 = 0.6;
+  goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(-M_PI);
+  goal.header.frame_id = "map";
+
+  auto always_cancelled = []() {return true;};
+
+  EXPECT_THROW(
+    obj->getPlan(start, goal, "GridBased", always_cancelled),
+    nav2_core::PlannerCancelled);
+  // obj->onCleanup(state);
+  obj.reset();
+}
+
 TEST(testPluginMap, Failures)
 {
   auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>();
@@ -147,6 +177,7 @@ TEST(testPluginMap, Failures)
   obj->onCleanup(state);
 }
 
+
 TEST(testPluginMap, Smac2dEqualStartGoal)
 {
   testSmallPathValidityAndOrientation("nav2_smac_planner/SmacPlanner2D", 0.0);
@@ -297,6 +328,32 @@ TEST(testPluginMap, ThetaStarAboveCostmapResolutionN)
   testSmallPathValidityAndNoOrientation("nav2_theta_star_planner/ThetaStarPlanner", 1.5);
 }
 
+TEST(testPluginMap, NavFnCancel)
+{
+  testCancel("nav2_navfn_planner/NavfnPlanner");
+}
+
+TEST(testPluginMap, ThetaStarCancel)
+{
+  testCancel("nav2_theta_star_planner/ThetaStarPlanner");
+}
+
+TEST(testPluginMap, Smac2dCancel)
+{
+  testCancel("nav2_smac_planner/SmacPlanner2D");
+}
+
+TEST(testPluginMap, SmacLatticeCancel)
+{
+  testCancel("nav2_smac_planner/SmacPlannerLattice");
+}
+
+TEST(testPluginMap, SmacHybridAStarCancel)
+{
+  testCancel("nav2_smac_planner/SmacPlannerHybrid");
+}
+
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);