halt the SwitchNode correctly

facontidavide · facontidavide · commit f9fec0afda35 · 2020-03-18T15:55:00.000+01:00
diff --git a/include/behaviortree_cpp_v3/controls/switch_node.h b/include/behaviortree_cpp_v3/controls/switch_node.h
@@ -61,6 +61,9 @@ class SwitchNode : public ControlNode
 template<size_t NUM_CASES> inline
 NodeStatus SwitchNode<NUM_CASES>::tick()
 {
+    constexpr const char * case_port_names[9] = {
+      "case_1", "case_2", "case_3", "case_4", "case_5", "case_6", "case_7", "case_8", "case_9"};
+
     if( childrenCount() != NUM_CASES+1)
     {
         throw LogicError("Wrong number of children in SwitchNode; "
@@ -76,10 +79,18 @@ NodeStatus SwitchNode<NUM_CASES>::tick()
         // check each case until you find a match
         for (unsigned index = 0; index < NUM_CASES; ++index)
         {
-            char case_str[20];
-            sprintf(case_str, "case_%d", index+1);
+            bool found = false;
+            if( index < 9 )
+            {
+                found = (bool)getInput(case_port_names[index], value);
+            }
+            else{
+                char case_str[20];
+                sprintf(case_str, "case_%d", index+1);
+                found = (bool)getInput(case_str, value);
+            }
 
-            if (getInput(case_str, value) && variable == value)
+            if (found && variable == value)
             {
                 child_index = index;
                 break;
@@ -90,18 +101,19 @@ NodeStatus SwitchNode<NUM_CASES>::tick()
     // if another one was running earlier, halt it
     if( running_child_ != -1 && running_child_ != child_index)
     {
-        halt();
+        children_nodes_[running_child_]->halt();
     }
 
-    NodeStatus ret = children_nodes_[child_index]->executeTick();
+    auto& selected_child = children_nodes_[child_index];
+    NodeStatus ret = selected_child->executeTick();
     if( ret == NodeStatus::RUNNING )
     {
         running_child_ = child_index;
     }
     else{
+        selected_child->halt();
         running_child_ = -1;
     }
-
     return ret;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -61,6 +61,9 @@ class SwitchNode : public ControlNode`
`61`	`61`	`template<size_t NUM_CASES> inline`
`62`	`62`	`NodeStatus SwitchNode<NUM_CASES>::tick()`
`63`	`63`	`{`
	`64`	`+ constexpr const char * case_port_names[9] = {`
	`65`	`+ "case_1", "case_2", "case_3", "case_4", "case_5", "case_6", "case_7", "case_8", "case_9"};`
	`66`	`+`
`64`	`67`	`if( childrenCount() != NUM_CASES+1)`
`65`	`68`	`{`
`66`	`69`	`throw LogicError("Wrong number of children in SwitchNode; "`
`@@ -76,10 +79,18 @@ NodeStatus SwitchNode<NUM_CASES>::tick()`
`76`	`79`	`// check each case until you find a match`
`77`	`80`	`for (unsigned index = 0; index < NUM_CASES; ++index)`
`78`	`81`	`{`
`79`		`- char case_str[20];`
`80`		`- sprintf(case_str, "case_%d", index+1);`
	`82`	`+ bool found = false;`
	`83`	`+ if( index < 9 )`
	`84`	`+ {`
	`85`	`+ found = (bool)getInput(case_port_names[index], value);`
	`86`	`+ }`
	`87`	`+ else{`
	`88`	`+ char case_str[20];`
	`89`	`+ sprintf(case_str, "case_%d", index+1);`
	`90`	`+ found = (bool)getInput(case_str, value);`
	`91`	`+ }`
`81`	`92`
`82`		`- if (getInput(case_str, value) && variable == value)`
	`93`	`+ if (found && variable == value)`
`83`	`94`	`{`
`84`	`95`	`child_index = index;`
`85`	`96`	`break;`
`@@ -90,18 +101,19 @@ NodeStatus SwitchNode<NUM_CASES>::tick()`
`90`	`101`	`// if another one was running earlier, halt it`
`91`	`102`	`if( running_child_ != -1 && running_child_ != child_index)`
`92`	`103`	`{`
`93`		`- halt();`
	`104`	`+ children_nodes_[running_child_]->halt();`
`94`	`105`	`}`
`95`	`106`
`96`		`- NodeStatus ret = children_nodes_[child_index]->executeTick();`
	`107`	`+ auto& selected_child = children_nodes_[child_index];`
	`108`	`+ NodeStatus ret = selected_child->executeTick();`
`97`	`109`	`if( ret == NodeStatus::RUNNING )`
`98`	`110`	`{`
`99`	`111`	`running_child_ = child_index;`
`100`	`112`	`}`
`101`	`113`	`else{`
	`114`	`+ selected_child->halt();`
`102`	`115`	`running_child_ = -1;`
`103`	`116`	`}`
`104`		`-`
`105`	`117`	`return ret;`
`106`	`118`	`}`
`107`	`119`