Behavior Tree uses Error Codes

nav2_behavior_tree/CMakeLists.txt

@@ -132,6 +132,12 @@ list(APPEND plugin_libs nav2_initial_pose_received_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_is_error_code_active_condition_bt_node SHARED plugins/condition/is_error_code_active_condition.cpp)
+list(APPEND plugin_libs nav2_is_error_code_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_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,19 @@ inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>(co
   return std::chrono::milliseconds(std::stoul(key.data()));
 }
 
+template<>
+inline std::set<int> convertFromString(StringView str)
+{
+  // We expect real numbers separated by commas
+  auto parts = splitString(str, ',');
+
+  std::set<int> vec_int;
+  for (const auto part : parts) {
+    vec_int.insert(convertFromString<int>(part));
+  }
+  return vec_int;
+}
+
 }  // namespace BT
 
 #endif  // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_error_code_active_condition.hpp

@@ -0,0 +1,71 @@
+// 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__IS_ERROR_CODE_ACTIVE_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_ERROR_CODE_ACTIVE_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 IsErrorCodeActive : public BT::ConditionNode
+{
+public:
+  IsErrorCodeActive(
+    const std::string & condition_name,
+    const BT::NodeConfiguration & conf)
+  : BT::ConditionNode(condition_name, conf)
+  {
+    getInput<std::set<int>>("error_codes_to_check", error_codes_to_check_);
+  }
+
+  IsErrorCodeActive() = delete;
+
+  BT::NodeStatus tick()
+  {
+    getInput<std::set<int>>("current_error_codes", current_error_codes_);
+
+    for (const auto & error_code_to_check : error_codes_to_check_) {
+      if (current_error_codes_.find(error_code_to_check) != current_error_codes_.end()) {
+        return BT::NodeStatus::SUCCESS;
+      }
+    }
+
+    return BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return
+      {
+        BT::InputPort<std::set<int>>("current_error_codes"),
+        BT::InputPort<std::set<int>>("error_codes_to_check")
+      };
+  }
+
+protected:
+  std::set<int> current_error_codes_;
+  std::set<int> error_codes_to_check_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_ERROR_CODE_ACTIVE_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/is_error_code_active_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class WouldAControllerRecoveryHelp : public IsErrorCodeActive
+{
+  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/plugins/condition/is_error_code_active_condition.cpp

@@ -0,0 +1,22 @@
+// 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/is_error_code_active_condition.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::IsErrorCodeActive>(
+    "IsErrorCodeActive");
+}

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)
+: IsErrorCodeActive(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/test/plugins/condition/CMakeLists.txt

@@ -41,3 +41,11 @@ ament_target_dependencies(test_condition_is_battery_low ${dependencies})
 ament_add_gtest(test_condition_is_path_valid test_is_path_valid.cpp)
 target_link_libraries(test_condition_is_path_valid nav2_is_path_valid_condition_bt_node)
 ament_target_dependencies(test_condition_is_path_valid ${dependencies})
+
+ament_add_gtest(test_is_error_code_active test_is_error_code_active.cpp)
+target_link_libraries(test_is_error_code_active nav2_is_error_code_active_condition_bt_node)
+ament_target_dependencies(test_is_error_code_active ${dependencies})
+
+ament_add_gtest(test_would_a_controller_recovery_help test_would_a_controller_recovery_help.cpp)
+target_link_libraries(test_would_a_controller_recovery_help nav2_would_a_controller_recovery_help_condition_bt_node)
+ament_target_dependencies(test_would_a_controller_recovery_help ${dependencies})

nav2_behavior_tree/test/plugins/condition/test_is_error_code_active.cpp

@@ -0,0 +1,86 @@
+// 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 <gtest/gtest.h>
+#include <memory>
+#include <map>
+
+#include "../../test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_error_code_active_condition.hpp"
+#include "nav2_msgs/action/follow_path.hpp"
+
+class IsErrorCodeActiveFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
+{
+public:
+  using Action = nav2_msgs::action::FollowPath;
+  using ActionGoal = Action::Goal;
+  void SetUp()
+  {
+    std::set<int> current_error_codes = {ActionGoal::NONE};
+    std::set<int> error_codes_to_check = {ActionGoal::UNKNOWN};
+    config_->blackboard->set("current_error_codes", current_error_codes);
+    config_->blackboard->set("error_codes_to_check", error_codes_to_check);
+
+    std::string xml_txt =
+      R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <IsErrorCodeActive current_error_codes="{current_error_codes}" error_codes_to_check="{error_codes_to_check}"/>
+        </BehaviorTree>
+      </root>)";
+
+    factory_->registerNodeType<nav2_behavior_tree::IsErrorCodeActive>(
+      "IsErrorCodeActive");
+    tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  }
+
+  void TearDown()
+  {
+    tree_.reset();
+  }
+
+protected:
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+std::shared_ptr<BT::Tree> IsErrorCodeActiveFixture::tree_ = nullptr;
+
+TEST_F(IsErrorCodeActiveFixture, test_condition)
+{
+  std::map<int, BT::NodeStatus> error_to_status_map = {
+    {ActionGoal::NONE, BT::NodeStatus::FAILURE},
+    {ActionGoal::UNKNOWN, BT::NodeStatus::SUCCESS},
+  };
+
+  for (const auto & error_to_status : error_to_status_map) {
+    std::set<int> error = {error_to_status.first};
+    config_->blackboard->set("current_error_codes", error);
+    EXPECT_EQ(tree_->tickRoot(), error_to_status.second);
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  bool all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}