nav2_system_tests: behaviour_tree error_code, error_msg checks (#4341)

Signed-off-by: Mike Wake <[email protected]>

nav2_system_tests/src/behavior_tree/server_handler.hpp

@@ -86,7 +86,15 @@ class DummyFollowPathActionServer : public DummyActionServer<nav2_msgs::action::
 {
 public:
   explicit DummyFollowPathActionServer(const rclcpp::Node::SharedPtr & node)
-  : DummyActionServer(node, "follow_path") {}
+  : DummyActionServer(node, "follow_path")
+  {
+    result_ = std::make_shared<nav2_msgs::action::FollowPath::Result>();
+  }
+
+  std::shared_ptr<nav2_msgs::action::FollowPath::Result> fillResult() override
+  {
+    return result_;
+  }
 
 protected:
   void updateResultForFailure(
@@ -96,6 +104,9 @@ class DummyFollowPathActionServer : public DummyActionServer<nav2_msgs::action::
     result->error_code = nav2_msgs::action::FollowPath::Result::NO_VALID_CONTROL;
     result->error_msg = "No valid control";
   }
+
+private:
+  std::shared_ptr<nav2_msgs::action::FollowPath::Result> result_;
 };
 
 

nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp

@@ -234,7 +234,12 @@ TEST_F(BehaviorTreeTestFixture, TestAllSuccess)
 
   // Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
+
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 1);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   // Goal count should be 0 since no goal is sent to all other servers
   EXPECT_EQ(server_handler->spin_server->getGoalCount(), 0);
@@ -285,9 +290,13 @@ TEST_F(BehaviorTreeTestFixture, TestAllFailure)
 
   // Goal count should be 2 since only two goals are sent to ComputePathToPose
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 14);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 207);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "Timeout");
 
   // Goal count should be 0 since no goal is sent to FollowPath action server
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   EXPECT_EQ(server_handler->spin_server->getGoalCount(), 5);
   EXPECT_EQ(server_handler->wait_server->getGoalCount(), 5);
@@ -334,7 +343,12 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries)
 
   // Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
+
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 2);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   // Navigate subtree recovery services are called once each
   EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 1);
@@ -392,9 +406,13 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoverySimple)
 
   // FollowPath is called 4 times
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 4);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   // ComputePathToPose is called 3 times
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
 
   // Local costmap is cleared 3 times
   EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);
@@ -486,9 +504,13 @@ TEST_F(BehaviorTreeTestFixture, TestNavigateRecoveryComplex)
 
   // ComputePathToPose is called 12 times
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 7);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
 
   // FollowPath is called 4 times
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 14);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   // Local costmap is cleared 5 times
   EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 9);
@@ -565,9 +587,13 @@ TEST_F(BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated)
 
   // ComputePathToPose is called 4 times
   EXPECT_EQ(server_handler->compute_path_to_pose_server->getGoalCount(), 3);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->compute_path_to_pose_server->fillResult()->error_msg, "");
 
   // FollowPath is called 3 times
   EXPECT_EQ(server_handler->follow_path_server->getGoalCount(), 5);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_code, 0);
+  EXPECT_EQ(server_handler->follow_path_server->fillResult()->error_msg, "");
 
   // Local costmap is cleared 2 times
   EXPECT_EQ(server_handler->clear_local_costmap_server->getRequestCount(), 3);

nav2_system_tests/src/error_codes/test_error_codes.py

@@ -80,7 +80,7 @@ def main(argv=sys.argv[1:]):
                 navigator.result_future.result().result.error_code == error_code
             ), 'Follow path error code does not match'
             assert (
-                navigator.result_future.result().result.error_msg != ""
+                navigator.result_future.result().result.error_msg != ''
             ), 'Follow path error_msg is empty'
 
         else:
@@ -115,7 +115,7 @@ def main(argv=sys.argv[1:]):
             result.error_code == error_code
         ), 'Compute path to pose error does not match'
         assert (
-            result.error_msg != ""
+            result.error_msg != ''
         ), 'Compute path to pose error_msg empty'
 
     def cancel_task():
@@ -152,7 +152,7 @@ def cancel_task():
             result.error_code == error_code
         ), 'Compute path through pose error does not match'
         assert (
-            result.error_msg != ""
+            result.error_msg != ''
         ), 'Compute path through pose error_msg is empty'
     # Check compute path to pose cancel
     threading.Thread(target=cancel_task).start()
@@ -191,7 +191,7 @@ def cancel_task():
     for smoother, error_code in smoother.items():
         result = navigator._smoothPathImpl(a_path, smoother)
         assert result.error_code == error_code, 'Smoother error does not match'
-        assert result.error_msg != "", 'Smoother error_msg is empty'
+        assert result.error_msg != '', 'Smoother error_msg is empty'
 
     navigator.lifecycleShutdown()
     rclpy.shutdown()

nav2_system_tests/src/gps_navigation/tester.py

@@ -190,9 +190,7 @@ def main(argv=sys.argv[1:]):
         test.action_result.missed_waypoints[0].error_code
         == ComputePathToPose.Result().GOAL_OUTSIDE_MAP
     )
-    assert (
-        test.action_result.missed_waypoints[0].error_msg != ""
-    )
+    assert (test.action_result.missed_waypoints[0].error_msg != '')
 
     # stop on failure test with bogous waypoint
     test.setStopFailureParam(True)

nav2_system_tests/src/waypoint_follower/tester.py

@@ -241,7 +241,7 @@ def main(argv=sys.argv[1:]):
         test.action_result.missed_waypoints[0].error_code
         == ComputePathToPose.Result().GOAL_OUTSIDE_MAP
     )
-    assert (test.action_result.missed_waypoints[0].error_msg != "")
+    assert (test.action_result.missed_waypoints[0].error_msg != '')
 
     # stop on failure test with bogous waypoint
     test.setStopFailureParam(True)