Added an option to repeat the previous selection in ManualSelector

daf@blue-ocean-robotics.com · daf@blue-ocean-robotics.com · commit 4a8cf33f3c20 · 2020-04-17T09:49:58.000+02:00
diff --git a/examples/t12_ncurses_manual_selector.cpp b/examples/t12_ncurses_manual_selector.cpp
@@ -3,17 +3,24 @@
 
 using namespace BT;
 
+/* Try also
+*      <ManualSelector repeat_last_selection="1">
+*  to see the difference.
+*/
+
 // clang-format off
 static const char* xml_text = R"(
  <root main_tree_to_execute = "MainTree" >
      <BehaviorTree ID="MainTree">
-        <ManualSelector name="root">
-            <SaySomething name="Option1"    message="Option1" />
-            <SaySomething name="Option2"    message="Option2" />
-            <SaySomething name="Option3"    message="Option3" />
-            <SaySomething name="Option4"    message="Option4" />
-            <ManualSelector name="YouChoose" />
-        </ManualSelector>
+        <Repeat num_cycles="3">
+            <ManualSelector repeat_last_selection="0">
+                <SaySomething name="Option1"    message="Option1" />
+                <SaySomething name="Option2"    message="Option2" />
+                <SaySomething name="Option3"    message="Option3" />
+                <SaySomething name="Option4"    message="Option4" />
+                <ManualSelector name="YouChoose" />
+            </ManualSelector>
+        </Repeat>
      </BehaviorTree>
  </root>
  )";
diff --git a/include/behaviortree_cpp_v3/controls/manual_node.h b/include/behaviortree_cpp_v3/controls/manual_node.h
@@ -23,16 +23,25 @@ namespace BT
 class ManualSelectorNode : public ControlNode
 {
   public:
-    ManualSelectorNode(const std::string& name);
+    ManualSelectorNode(const std::string& name, const NodeConfiguration& config);
 
     virtual ~ManualSelectorNode() override = default;
 
     virtual void halt() override;
 
+    static PortsList providedPorts()
+    {
+        return { InputPort<bool>(REPEAT_LAST_SELECTION, false,
+                                 "If true, execute again the same child that was selected the last time") };
+    }
+
   private:
 
+    static constexpr const char* REPEAT_LAST_SELECTION = "repeat_last_selection";
+
     virtual BT::NodeStatus tick() override;
     int running_child_idx_;
+    int previously_executed_idx_;
 
     enum NumericarStatus{
         NUM_SUCCESS = 253,
diff --git a/src/controls/manual_node.cpp b/src/controls/manual_node.cpp
@@ -19,9 +19,10 @@ namespace BT
 {
 
 
-ManualSelectorNode::ManualSelectorNode(const std::string& name)
-    : ControlNode::ControlNode(name, {} )
+ManualSelectorNode::ManualSelectorNode(const std::string& name, const NodeConfiguration& config)
+    : ControlNode::ControlNode(name, config )
   , running_child_idx_(-1)
+  , previously_executed_idx_(-1)
 {
     setRegistrationID("ManualSelector");
 }
@@ -45,22 +46,32 @@ NodeStatus ManualSelectorNode::tick()
         return selectStatus();
     }
 
-    setStatus(NodeStatus::RUNNING);
+    bool repeat_last = false;
+    getInput(REPEAT_LAST_SELECTION, repeat_last);
 
-    unsigned idx = selectChild();
+    int idx = 0;
 
-    if( idx == NUM_SUCCESS ){
-        return NodeStatus::SUCCESS;
-    }
-    if( idx == NUM_FAILURE ){
-        return NodeStatus::FAILURE;
+    if( repeat_last && previously_executed_idx_ >= 0)
+    {
+        idx = previously_executed_idx_;
     }
-    if( idx == NUM_RUNNING ){
-        return NodeStatus::RUNNING;
+    else{
+        setStatus(NodeStatus::RUNNING);
+        idx = selectChild();
+        previously_executed_idx_ = idx;
+
+        if( idx == NUM_SUCCESS ){
+            return NodeStatus::SUCCESS;
+        }
+        if( idx == NUM_FAILURE ){
+            return NodeStatus::FAILURE;
+        }
+        if( idx == NUM_RUNNING ){
+            return NodeStatus::RUNNING;
+        }
     }
 
     NodeStatus ret = children_nodes_[idx]->executeTick();
-
     if(ret == NodeStatus::RUNNING)
     {
         running_child_idx_ = idx;
