fix issue #710

Author: facontidavide

include/behaviortree_cpp/controls/switch_node.h

@@ -36,47 +36,73 @@ namespace BT
 When the SwitchNode is executed (Switch3 is a node with 3 cases)
 the "variable" will be compared to the cases and execute the correct child
 or the default one (last).
-
  *
  */
+
+namespace details {
+
+bool CheckStringEquality(const std::string& v1, const std::string& v2,
+                         const ScriptingEnumsRegistry *enums);
+}
+
 template <size_t NUM_CASES>
 class SwitchNode : public ControlNode
 {
 public:
-  SwitchNode(const std::string& name, const BT::NodeConfig& config) :
-    ControlNode::ControlNode(name, config), running_child_(-1)
-  {
-    setRegistrationID("Switch");
-  }
+  SwitchNode(const std::string& name, const BT::NodeConfig& config);
 
   virtual ~SwitchNode() override = default;
 
-  void halt() override
+  void halt() override;
+
+  static PortsList providedPorts();
+
+private:
+  int running_child_;
+  std::vector<std::string> case_keys_;
+  virtual BT::NodeStatus tick() override;
+};
+
+//-----------------------------------------------
+//-----------------------------------------------
+
+template<size_t NUM_CASES> inline
+SwitchNode<NUM_CASES>::SwitchNode(const std::string &name, const NodeConfig &config) :
+    ControlNode::ControlNode(name, config), running_child_(-1)
+{
+  setRegistrationID("Switch");
+  for (unsigned i = 1; i <= NUM_CASES; i++)
   {
-    running_child_ = -1;
-    ControlNode::halt();
+    case_keys_.push_back( std::string("case_") + std::to_string(i));
   }
+}
 
-  static PortsList providedPorts()
-  {
+template<size_t NUM_CASES> inline
+void SwitchNode<NUM_CASES>::halt()
+{
+  running_child_ = -1;
+  ControlNode::halt();
+}
+
+template<size_t NUM_CASES> inline
+PortsList SwitchNode<NUM_CASES>::providedPorts()
+{
+  static PortsList ports = []() {
     PortsList ports;
     ports.insert(BT::InputPort<std::string>("variable"));
-    for (unsigned i = 0; i < NUM_CASES; i++)
+    for (unsigned i = 1; i <= NUM_CASES; i++)
     {
-      char case_str[20];
-      sprintf(case_str, "case_%d", i + 1);
-      ports.insert(BT::InputPort<std::string>(case_str));
+      auto key = std::string("case_") + std::to_string(i);
+      ports.insert(BT::InputPort<std::string>(key));
     }
     return ports;
-  }
+  }();
 
-private:
-  int running_child_;
-  virtual BT::NodeStatus tick() override;
-};
+  return ports;
+}
 
-template <size_t NUM_CASES>
-inline NodeStatus SwitchNode<NUM_CASES>::tick()
+template <size_t NUM_CASES> inline
+    NodeStatus SwitchNode<NUM_CASES>::tick()
 {
   if (childrenCount() != NUM_CASES + 1)
   {
@@ -88,19 +114,21 @@ inline NodeStatus SwitchNode<NUM_CASES>::tick()
   std::string value;
   int match_index = int(NUM_CASES);   // default index;
 
-  if (getInput("variable", variable))   // no variable? jump to default
+  // no variable? jump to default
+  if (getInput("variable", variable))
   {
     // check each case until you find a match
     for (int index = 0; index < int(NUM_CASES); ++index)
     {
-      char case_key[20];
-      sprintf(case_key, "case_%d", int(index + 1));
-      bool found = static_cast<bool>(getInput(case_key, value));
-
-      if (found && variable == value)
+      const std::string& case_key = case_keys_[index];
+      if (getInput(case_key, value))
       {
-        match_index = index;
-        break;
+        if(details::CheckStringEquality(
+                variable, value, this->config().enums.get()))
+        {
+          match_index = index;
+          break;
+        }
       }
     }
   }

src/controls/switch_node.cpp

@@ -11,3 +11,54 @@
 */
 
 #include "behaviortree_cpp/controls/switch_node.h"
+
+namespace BT::details
+{
+
+bool CheckStringEquality(const std::string &v1, const std::string &v2,
+                         const ScriptingEnumsRegistry* enums)
+{
+  // compare strings first
+  if(v1 == v2)
+  {
+    return true;
+  }
+  // compare as integers next
+  auto ToInt = [enums](const std::string& str, auto& result) -> bool
+  {
+    if(enums)
+    {
+      auto it = enums->find(str);
+      if( it != enums->end())
+      {
+        result = it->second;
+        return true;
+      }
+    }
+    auto [ptr, ec] = std::from_chars(str.data(), str.data() + str.size(), result);
+    return (ec == std::errc());
+  };
+  int v1_int = 0;
+  int v2_int = 0;
+  if(ToInt(v1, v1_int) && ToInt(v2, v2_int) && v1_int == v2_int)
+  {
+    return true;
+  }
+  // compare as real numbers next
+  auto ToReal = [](const std::string& str, auto& result) -> bool
+  {
+    auto [ptr, ec] = std::from_chars(str.data(), str.data() + str.size(), result);
+    return (ec == std::errc());
+  };
+  double v1_real = 0;
+  double v2_real = 0;
+  constexpr auto eps = double(std::numeric_limits<float>::epsilon());
+  if(ToReal(v1, v1_real) && ToReal(v2, v2_real) &&
+      std::abs(v1_real - v2_real) <= eps)
+  {
+    return true;
+  }
+  return false;
+}
+
+}

tests/gtest_enums.cpp

@@ -89,3 +89,35 @@ TEST(Enums, StrintToEnum)
   }
 }
 
+TEST(Enums, SwitchNodeWithEnum)
+{
+  const std::string xml_txt = R"(
+    <root BTCPP_format="4" >
+      <BehaviorTree ID="Main">
+        <Sequence>
+          <Script code=" my_color := Blue "/>
+          <Switch4 variable="{my_color}"
+            case_1="Red"
+            case_2="Blue"
+            case_3="Green"
+            case_4="Undefined">
+            <AlwaysFailure name="case_red" />
+            <AlwaysSuccess name="case_blue" />
+            <AlwaysFailure name="case_green" />
+            <AlwaysFailure name="case_undefined" />
+            <AlwaysFailure name="default_case" />
+          </Switch4>
+        </Sequence>
+      </BehaviorTree>
+    </root>)";
+
+  BehaviorTreeFactory factory;
+  factory.registerScriptingEnums<Color>();
+
+  auto tree = factory.createTreeFromText(xml_txt);
+
+  NodeStatus status = tree.tickWhileRunning();
+
+  ASSERT_EQ(status, NodeStatus::SUCCESS);
+}
+