From f3a041a7d7081ab35d6c4fcd208d0949829a77e1 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Thu, 7 Sep 2023 20:41:35 +0300 Subject: [PATCH] Fixes for flaky WPF test (#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 * Clean error code in any situation * Fix UNKNOWN WPF status handling --------- Co-authored-by: Steve Macenski --- .../include/nav2_behavior_tree/bt_action_server.hpp | 5 +++++ .../nav2_behavior_tree/bt_action_server_impl.hpp | 11 +++++++++++ nav2_common/nav2_common/launch/rewritten_yaml.py | 10 ++++++++++ .../src/waypoint_follower/test_case_py.launch | 5 ++++- nav2_system_tests/src/waypoint_follower/tester.py | 2 -- nav2_waypoint_follower/src/waypoint_follower.cpp | 10 ++++++---- 6 files changed, 36 insertions(+), 7 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 79b1df5bb4..2dcb21e42b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -196,6 +196,11 @@ class BtActionServer */ void populateErrorCode(typename std::shared_ptr result); + /** + * @brief Setting BT error codes to success. Used to clean blackboard between different BT runs + */ + void cleanErrorCodes(); + // Action name std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 9a8ddc613c..7276429179 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -236,6 +236,7 @@ void BtActionServer::executeCallback() { if (!on_goal_received_callback_(action_server_->get_current_goal())) { action_server_->terminate_current(); + cleanErrorCodes(); return; } @@ -290,6 +291,8 @@ void BtActionServer::executeCallback() action_server_->terminate_all(result); break; } + + cleanErrorCodes(); } template @@ -316,6 +319,14 @@ void BtActionServer::populateErrorCode( } } +template +void BtActionServer::cleanErrorCodes() +{ + for (const auto & error_code : error_code_names_) { + blackboard_->set(error_code, 0); //NOLINT + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 2887fa5503..071c884864 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/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: diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index adeb839c48..51ccbb451e 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/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() diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index d2083fbb4f..30dfff574b 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/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 diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index fb214bd41c..21dbf3caae 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/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; }