Fixes for flaky WPF test (ros-navigation#3785)

* Fixes for flaky WPF test:
* New RewrittenYaml ability to add non-existing parameters
* Prune distance fix for WPF test
* Treat UNKNOWN status as error in WPF
* Clear error codes after BT run
* Remove unnecessary setInitialPose() from WPF test

* Update nav2_waypoint_follower/src/waypoint_follower.cpp

Co-authored-by: Steve Macenski <[email protected]>

* Clean error code in any situation

* Fix UNKNOWN WPF status handling

---------

Co-authored-by: Steve Macenski <[email protected]>
Signed-off-by: enricosutera <[email protected]>

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -196,6 +196,11 @@ class BtActionServer
    */
   void populateErrorCode(typename std::shared_ptr<typename ActionT::Result> result);
 
+  /**
+   * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs
+   */
+  void cleanErrorCodes();
+
   // Action name
   std::string action_name_;
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -236,6 +236,7 @@ void BtActionServer<ActionT>::executeCallback()
 {
   if (!on_goal_received_callback_(action_server_->get_current_goal())) {
     action_server_->terminate_current();
+    cleanErrorCodes();
     return;
   }
 
@@ -290,6 +291,8 @@ void BtActionServer<ActionT>::executeCallback()
       action_server_->terminate_all(result);
       break;
   }
+
+  cleanErrorCodes();
 }
 
 template<class ActionT>
@@ -316,6 +319,14 @@ void BtActionServer<ActionT>::populateErrorCode(
   }
 }
 
+template<class ActionT>
+void BtActionServer<ActionT>::cleanErrorCodes()
+{
+  for (const auto & error_code : error_code_names_) {
+    blackboard_->set<unsigned short>(error_code, 0);  //NOLINT
+  }
+}
+
 }  // namespace nav2_behavior_tree
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -87,6 +87,7 @@ def perform(self, context: launch.LaunchContext) -> Text:
         param_rewrites, keys_rewrites = self.resolve_rewrites(context)
         data = yaml.safe_load(open(yaml_filename, 'r'))
         self.substitute_params(data, param_rewrites)
+        self.add_params(data, param_rewrites)
         self.substitute_keys(data, keys_rewrites)
         if self.__root_key is not None:
             root_key = launch.utilities.perform_substitutions(context, self.__root_key)
@@ -121,6 +122,15 @@ def substitute_params(self, yaml, param_rewrites):
                 yaml_keys = path.split('.')
                 yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val)
 
+    def add_params(self, yaml, param_rewrites):
+        # add new total path parameters
+        yaml_paths = self.pathify(yaml)
+        for path in param_rewrites:
+            if not path in yaml_paths:
+                new_val = self.convert(param_rewrites[path])
+                yaml_keys = path.split('.')
+                if 'ros__parameters' in yaml_keys:
+                    yaml = self.updateYamlPathVals(yaml, yaml_keys, new_val)
 
     def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
         for key in yaml_key_list:

nav2_system_tests/src/waypoint_follower/test_case_py.launch

@@ -41,10 +41,13 @@ def generate_launch_description():
     params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml')
 
     # Replace the `use_astar` setting on the params file
+    param_substitutions = {
+        'controller_server.ros__parameters.FollowPath.prune_distance': '1.0',
+        'controller_server.ros__parameters.FollowPath.forward_prune_distance': '1.0'}
     configured_params = RewrittenYaml(
         source_file=params_file,
         root_key='',
-        param_rewrites='',
+        param_rewrites=param_substitutions,
         convert_types=True)
 
     context = LaunchContext()

nav2_system_tests/src/waypoint_follower/tester.py

@@ -236,9 +236,7 @@ def main(argv=sys.argv[1:]):
     # stop on failure test with bogous waypoint
     test.setStopFailureParam(True)
     bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]]
-    starting_pose = [-2.0, -0.5]
     test.setWaypoints(bwps)
-    test.setInitialPose(starting_pose)
     result = test.run(True, False)
     assert not result
     result = not result

nav2_waypoint_follower/src/waypoint_follower.cpp

@@ -217,7 +217,10 @@ WaypointFollower::followWaypoints()
     feedback->current_waypoint = goal_index;
     action_server_->publish_feedback(feedback);
 
-    if (current_goal_status_.status == ActionStatus::FAILED) {
+    if (
+      current_goal_status_.status == ActionStatus::FAILED ||
+      current_goal_status_.status == ActionStatus::UNKNOWN)
+    {
       nav2_msgs::msg::MissedWaypoint missedWaypoint;
       missedWaypoint.index = goal_index;
       missedWaypoint.goal = goal->poses[goal_index];
@@ -272,9 +275,7 @@ WaypointFollower::followWaypoints()
       }
     }
 
-    if (current_goal_status_.status != ActionStatus::PROCESSING &&
-      current_goal_status_.status != ActionStatus::UNKNOWN)
-    {
+    if (current_goal_status_.status != ActionStatus::PROCESSING) {
       // Update server state
       goal_index++;
       new_goal = true;
@@ -324,6 +325,7 @@ WaypointFollower::resultCallback(
       current_goal_status_.status = ActionStatus::FAILED;
       return;
     default:
+      RCLCPP_ERROR(get_logger(), "Received an UNKNOWN result code from navigation action!");
       current_goal_status_.status = ActionStatus::UNKNOWN;
       return;
   }