Behavior Tree uses Error Codes (ros-navigation#3324)

* rough outline for condition node

* completed error code condition

* behavior tree with error codes

* created generic code ex

* test for error_code_condition

* generic error code bt node

* remove error code condition

* updates

* updated error code condition

* would a controller recovery help

* rename

* added planner recovery condition

* initial draft

* complete with one error code as input

* revert cmake

* bt conversion test

* code review

* code review

* code review

* refactor behavior tree tests

* cleanup

* final cleanup

* uncomment

* removed logger

* function header update

* update bt to include would a planner recovery help

* copyright cleanup

* added bt node for smoother recovery

* smoother test

* costmap filter test fix

* remove include

* test if commit counted

* update copyright

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>

nav2_behavior_tree/CMakeLists.txt

@@ -135,6 +135,18 @@ list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node)
 add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
 list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
 
+add_library(nav2_are_error_codes_active_condition_bt_node SHARED plugins/condition/are_error_codes_present_condition.cpp)
+list(APPEND plugin_libs nav2_are_error_codes_active_condition_bt_node)
+
+add_library(nav2_would_a_controller_recovery_help_condition_bt_node SHARED plugins/condition/would_a_controller_recovery_help_condition.cpp)
+list(APPEND plugin_libs nav2_would_a_controller_recovery_help_condition_bt_node)
+
+add_library(nav2_would_a_planner_recovery_help_condition_bt_node SHARED plugins/condition/would_a_planner_recovery_help_condition.cpp)
+list(APPEND plugin_libs nav2_would_a_planner_recovery_help_condition_bt_node)
+
+add_library(nav2_would_a_smoother_recovery_help_condition_bt_node SHARED plugins/condition/would_a_smoother_recovery_help_condition.cpp)
+list(APPEND plugin_libs nav2_would_a_smoother_recovery_help_condition_bt_node)
+
 add_library(nav2_reinitialize_global_localization_service_bt_node SHARED plugins/action/reinitialize_global_localization_service.cpp)
 list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp

@@ -16,6 +16,7 @@
 #define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
 
 #include <string>
+#include <set>
 
 #include "rclcpp/time.hpp"
 #include "behaviortree_cpp_v3/behavior_tree.h"
@@ -111,6 +112,24 @@ inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(co
   return std::chrono::milliseconds(std::stoul(key.data()));
 }
 
+/**
+ * @brief Parse XML string to std::set<int>
+ * @param key XML string
+ * @return std::set<int>
+ */
+template<>
+inline std::set<int> convertFromString(StringView key)
+{
+  // Real numbers separated by semicolons
+  auto parts = splitString(key, ';');
+
+  std::set<int> set;
+  for (const auto part : parts) {
+    set.insert(convertFromString<int>(part));
+  }
+  return set;
+}
+
 }  // namespace BT
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_ERROR_CODES_PRESENT_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_ERROR_CODES_PRESENT_CONDITION_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <set>
+
+#include "rclcpp/rclcpp.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+class AreErrorCodesPresent : public BT::ConditionNode
+{
+public:
+  AreErrorCodesPresent(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf)
+  : BT::ConditionNode(condition_name, conf)
+  {
+    getInput<std::set<unsigned short>>("error_codes_to_check", error_codes_to_check_); //NOLINT
+  }
+
+  AreErrorCodesPresent() = delete;
+
+  BT::NodeStatus tick()
+  {
+    getInput<unsigned short>("error_code", error_code_);  //NOLINT
+
+    if (error_codes_to_check_.find(error_code_) != error_codes_to_check_.end()) {
+      return BT::NodeStatus::SUCCESS;
+    }
+
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return
+      {
+        BT::InputPort<unsigned short>("error_code", "The active error codes"), //NOLINT
+        BT::InputPort<std::set<unsigned short>>("error_codes_to_check", "Error codes to check")//NOLINT
+      };
+  }
+
+protected:
+  unsigned short error_code_; //NOLINT
+  std::set<unsigned short> error_codes_to_check_; //NOLINT
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__ARE_ERROR_CODES_PRESENT_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_CONTROLLER_RECOVERY_HELP_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_CONTROLLER_RECOVERY_HELP_CONDITION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class WouldAControllerRecoveryHelp : public AreErrorCodesPresent
+{
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionGoal = Action::Goal;
+
+public:
+  WouldAControllerRecoveryHelp(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  WouldAControllerRecoveryHelp() = delete;
+};
+
+}  // namespace nav2_behavior_tree
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_CONTROLLER_RECOVERY_HELP_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_PLANNER_RECOVERY_HELP_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_PLANNER_RECOVERY_HELP_CONDITION_HPP_
+
+#include <string>
+
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class WouldAPlannerRecoveryHelp : public AreErrorCodesPresent
+{
+  using Action = nav2_msgs::action::ComputePathToPose;
+  using ActionGoal = Action::Goal;
+
+public:
+  WouldAPlannerRecoveryHelp(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  WouldAPlannerRecoveryHelp() = delete;
+};
+
+}  // namespace nav2_behavior_tree
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_PLANNER_RECOVERY_HELP_CONDITION_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp

@@ -0,0 +1,42 @@
+// Copyright (c) 2023 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+//  Unless required by applicable law or agreed to in writing, software
+//  distributed under the License is distributed on an "AS IS" BASIS,
+//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+//  See the License for the specific language governing permissions and
+//  limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_SMOOTHER_RECOVERY_HELP_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_SMOOTHER_RECOVERY_HELP_CONDITION_HPP_
+
+
+#include <string>
+
+#include "nav2_msgs/action/smooth_path.hpp"
+#include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class WouldASmootherRecoveryHelp : public AreErrorCodesPresent
+{
+  using Action = nav2_msgs::action::SmoothPath;
+  using ActionGoal = Action::Goal;
+
+public:
+  WouldASmootherRecoveryHelp(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf);
+
+  WouldASmootherRecoveryHelp() = delete;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__WOULD_A_SMOOTHER_RECOVERY_HELP_CONDITION_HPP_

nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.cpp

@@ -0,0 +1,22 @@
+// Copyright (c) 2023 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/condition/are_error_codes_present_condition.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::AreErrorCodesPresent>(
+    "AreErrorCodesPresent");
+}

nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.cpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/condition/would_a_controller_recovery_help_condition.hpp"
+#include <memory>
+
+namespace nav2_behavior_tree
+{
+
+WouldAControllerRecoveryHelp::WouldAControllerRecoveryHelp(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: AreErrorCodesPresent(condition_name, conf)
+{
+  error_codes_to_check_ = {
+    ActionGoal::UNKNOWN,
+    ActionGoal::PATIENCE_EXCEEDED,
+    ActionGoal::FAILED_TO_MAKE_PROGRESS,
+    ActionGoal::NO_VALID_CONTROL
+  };
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::WouldAControllerRecoveryHelp>(
+    "WouldAControllerRecoveryHelp");
+}

nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.cpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2022 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/condition/would_a_planner_recovery_help_condition.hpp"
+#include <memory>
+
+namespace nav2_behavior_tree
+{
+
+WouldAPlannerRecoveryHelp::WouldAPlannerRecoveryHelp(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: AreErrorCodesPresent(condition_name, conf)
+{
+  error_codes_to_check_ = {
+    ActionGoal::UNKNOWN,
+    ActionGoal::NO_VALID_PATH,
+    ActionGoal::TIMEOUT
+  };
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::WouldAPlannerRecoveryHelp>(
+    "WouldAPlannerRecoveryHelp");
+}

nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.cpp

@@ -0,0 +1,41 @@
+// Copyright (c) 2023 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_behavior_tree/plugins/condition/would_a_smoother_recovery_help_condition.hpp"
+#include <memory>
+
+namespace nav2_behavior_tree
+{
+
+WouldASmootherRecoveryHelp::WouldASmootherRecoveryHelp(
+  const std::string & condition_name,
+  const BT::NodeConfiguration & conf)
+: AreErrorCodesPresent(condition_name, conf)
+{
+  error_codes_to_check_ = {
+    ActionGoal::UNKNOWN,
+    ActionGoal::TIMEOUT,
+    ActionGoal::FAILED_TO_SMOOTH_PATH,
+    ActionGoal::SMOOTHED_PATH_IN_COLLISION
+  };
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::WouldASmootherRecoveryHelp>(
+    "WouldASmootherRecoveryHelp");
+}