backporting changes from v4.x

BehaviorTree · Sep 28, 2022 · cc77171 · cc77171
1 parent d23578b
commit cc77171
Show file tree

Hide file tree

Showing 42 changed files with 156 additions and 191 deletions.
diff --git a/include/behaviortree_cpp_v3/action_node.h b/include/behaviortree_cpp_v3/action_node.h
@@ -25,10 +25,9 @@ namespace BT
 {
 
 // IMPORTANT: Actions which returned SUCCESS or FAILURE will not be ticked
-// again unless setStatus(IDLE) is called first.
+// again unless resetStatus() is called first.
 // Keep this in mind when writing your custom Control and Decorator nodes.
 
-
 /**
  * @brief The ActionNodeBase is the base class to use to create any kind of action.
  * A particular derived class is free to override executeTick() as needed.
@@ -64,9 +63,7 @@ class SyncActionNode : public ActionNodeBase
 
     /// You don't need to override this
     virtual void halt() override final
-    {
-        setStatus(NodeStatus::IDLE);
-    }
+    { }
 };
 
 /**
@@ -140,7 +137,7 @@ class AsyncActionNode : public ActionNodeBase
     std::exception_ptr exptr_;
     std::atomic_bool halt_requested_;
     std::future<void> thread_handle_;
-    std::mutex m_;
+    std::mutex mutex_;
 };
 
 /**

diff --git a/include/behaviortree_cpp_v3/actions/always_failure_node.h b/include/behaviortree_cpp_v3/actions/always_failure_node.h
@@ -10,8 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef ACTION_ALWAYS_FAILURE_NODE_H
-#define ACTION_ALWAYS_FAILURE_NODE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/action_node.h"
 
@@ -37,4 +36,3 @@ class AlwaysFailureNode : public SyncActionNode
 };
 }
 
-#endif
diff --git a/include/behaviortree_cpp_v3/actions/always_success_node.h b/include/behaviortree_cpp_v3/actions/always_success_node.h
@@ -10,8 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef ACTION_ALWAYS_SUCCESS_NODE_H
-#define ACTION_ALWAYS_SUCCESS_NODE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/action_node.h"
 
@@ -37,4 +36,3 @@ class AlwaysSuccessNode : public SyncActionNode
 };
 }
 
-#endif
diff --git a/include/behaviortree_cpp_v3/actions/pop_from_queue.hpp b/include/behaviortree_cpp_v3/actions/pop_from_queue.hpp
@@ -1,4 +1,4 @@
-/*  Copyright (C) 2018-2022 Davide Faconti, Eurecat -  All Rights Reserved
+/*  Copyright (C) 2022 Davide Faconti -  All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
@@ -10,9 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-
-#ifndef BEHAVIORTREE_POPFROMQUEUE_HPP
-#define BEHAVIORTREE_POPFROMQUEUE_HPP
+#pragma once
 
 #include <list>
 #include <mutex>
@@ -145,4 +143,3 @@ class QueueSize : public SyncActionNode
 
 }
 
-#endif // BEHAVIORTREE_POPFROMQUEUE_HPP
diff --git a/include/behaviortree_cpp_v3/basic_types.h b/include/behaviortree_cpp_v3/basic_types.h
@@ -40,6 +40,12 @@ enum class NodeStatus
     FAILURE
 };
 
+inline bool StatusCompleted(const NodeStatus& status)
+{
+    return status == NodeStatus::SUCCESS ||
+           status == NodeStatus::FAILURE;
+}
+
 enum class PortDirection{
     INPUT,
     OUTPUT,

diff --git a/include/behaviortree_cpp_v3/behavior_tree.h b/include/behaviortree_cpp_v3/behavior_tree.h
@@ -89,6 +89,7 @@ inline NodeType getType()
     return NodeType::UNDEFINED;
     // clang-format on
 }
+
 }
 
 #endif   // BEHAVIOR_TREE_H
diff --git a/include/behaviortree_cpp_v3/blackboard.h b/include/behaviortree_cpp_v3/blackboard.h
@@ -51,7 +51,7 @@ class Blackboard
     const Any* getAny(const std::string& key) const
     {
         std::unique_lock<std::mutex> lock(mutex_);
-
+        // search first if this port was remapped
         if( auto parent = parent_bb_.lock())
         {
             auto remapping_it = internal_to_external_.find(key);
@@ -67,7 +67,7 @@ class Blackboard
     Any* getAny(const std::string& key)
     {
         std::unique_lock<std::mutex> lock(mutex_);
-
+        // search first if this port was remapped
         if( auto parent = parent_bb_.lock())
         {
             auto remapping_it = internal_to_external_.find(key);
@@ -118,39 +118,33 @@ class Blackboard
     {
         std::unique_lock<std::mutex> lock_entry(entry_mutex_);
         std::unique_lock<std::mutex> lock(mutex_);
-        auto it = storage_.find(key);
 
-        if( auto parent = parent_bb_.lock())
+        // search first if this port was remapped.
+        // Change the parent_bb_ in that case
+        auto remapping_it = internal_to_external_.find(key);
+        if( remapping_it != internal_to_external_.end())
         {
-            auto remapping_it = internal_to_external_.find(key);
-            if( remapping_it != internal_to_external_.end())
+            const auto& remapped_key = remapping_it->second;
+            if( auto parent = parent_bb_.lock())
             {
-                const auto& remapped_key = remapping_it->second;
-                if( it == storage_.end() ) // virgin entry
-                {
-                    auto parent_info = parent->portInfo(remapped_key);
-                    if( parent_info )
-                    {
-                        storage_.emplace( key, Entry( *parent_info ) );
-                    }
-                    else{
-                        storage_.emplace( key, Entry( PortInfo() ) );
-                    }
-                }
                 parent->set( remapped_key, value );
                 return;
             }
         }
 
-        if( it != storage_.end() ) // already there. check the type
+        // check local storage
+        auto it = storage_.find(key);
+        if( it != storage_.end() )
         {
             const PortInfo& port_info = it->second.port_info;
             auto& previous_any = it->second.value;
-            const auto locked_type = port_info.type();
+            const auto previous_type = port_info.type();
 
-            Any temp(value);
+            Any new_value(value);
 
-            if( locked_type && *locked_type != typeid(T) && *locked_type != temp.type() )
+            if( previous_type &&
+                *previous_type != typeid(T) &&
+                *previous_type != new_value.type() )
             {
                 bool mismatching = true;
                 if( std::is_constructible<StringView, T>::value )
@@ -159,7 +153,7 @@ class Blackboard
                     if( any_from_string.empty() == false)
                     {
                         mismatching = false;
-                        temp = std::move( any_from_string );
+                        new_value = std::move( any_from_string );
                     }
                 }
 
@@ -168,11 +162,11 @@ class Blackboard
                     debugMessage();
 
                     throw LogicError( "Blackboard::set() failed: once declared, the type of a port shall not change. "
-                                     "Declared type [", demangle( locked_type ),
-                                     "] != current type [", demangle( typeid(T) ),"]" );
+                                     "Declared type [", BT::demangle(previous_type),
+                                     "] != current type [", BT::demangle(typeid(T)),"]" );
                 }
             }
-            previous_any = std::move(temp);
+            previous_any = std::move(new_value);
         }
         else{ // create for the first time without any info
             storage_.emplace( key, Entry( Any(value), PortInfo() ) );
@@ -201,7 +195,7 @@ class Blackboard
     // done using the value.
     std::mutex& entryMutex()
     {
-      return entry_mutex_;
+        return entry_mutex_;
     }
 
   private:

diff --git a/include/behaviortree_cpp_v3/bt_parser.h b/include/behaviortree_cpp_v3/bt_parser.h
@@ -1,5 +1,17 @@
-#ifndef PARSING_BT_H
-#define PARSING_BT_H
+/* Copyright (C) 2022 Davide Faconti -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+
+#pragma once
 
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/blackboard.h"
@@ -32,4 +44,3 @@ class Parser
 
 }
 
-#endif   // PARSING_BT_H
diff --git a/include/behaviortree_cpp_v3/condition_node.h b/include/behaviortree_cpp_v3/condition_node.h
@@ -26,10 +26,7 @@ class ConditionNode : public LeafNode
     virtual ~ConditionNode() override = default;
 
     //Do nothing
-    virtual void halt() override final
-    {
-        setStatus(NodeStatus::IDLE);
-    }
+    virtual void halt() override final {}
 
     virtual NodeType type() const override final
     {

diff --git a/include/behaviortree_cpp_v3/control_node.h b/include/behaviortree_cpp_v3/control_node.h
@@ -11,8 +11,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef CONTROLNODE_H
-#define CONTROLNODE_H
+#pragma once
 
 #include <vector>
 #include "behaviortree_cpp_v3/tree_node.h"
@@ -57,4 +56,3 @@ class ControlNode : public TreeNode
 };
 }
 
-#endif
diff --git a/include/behaviortree_cpp_v3/controls/fallback_node.h b/include/behaviortree_cpp_v3/controls/fallback_node.h
@@ -11,8 +11,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef FALLBACKNODE_H
-#define FALLBACKNODE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/control_node.h"
 
@@ -47,4 +46,3 @@ class FallbackNode : public ControlNode
 
 }
 
-#endif
diff --git a/include/behaviortree_cpp_v3/controls/if_then_else_node.h b/include/behaviortree_cpp_v3/controls/if_then_else_node.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+/* Copyright (C) 2020-2022 Davide Faconti -  All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
@@ -10,8 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef BT_IF_THEN_ELSE_H
-#define BT_IF_THEN_ELSE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/control_node.h"
 
@@ -49,4 +48,3 @@ class IfThenElseNode : public ControlNode
 
 }
 
-#endif // BT_IF_THEN_ELSE_H
diff --git a/include/behaviortree_cpp_v3/controls/manual_node.h b/include/behaviortree_cpp_v3/controls/manual_node.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2020 Davide Faconti -  All Rights Reserved
+/* Copyright (C) 2020-2022 Davide Faconti -  All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
@@ -10,8 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef MANUAL_SELECTION_NODE_H
-#define MANUAL_SELECTION_NODE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/control_node.h"
 
@@ -56,4 +55,3 @@ class ManualSelectorNode : public ControlNode
 
 }
 
-#endif // MANUAL_SELECTION_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/parallel_node.h b/include/behaviortree_cpp_v3/controls/parallel_node.h
@@ -1,5 +1,5 @@
 /* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
- * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+ * Copyright (C) 2018-2022 Davide Faconti, Eurecat -  All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
@@ -11,8 +11,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef PARALLEL_NODE_H
-#define PARALLEL_NODE_H
+#pragma once
 
 #include <set>
 #include "behaviortree_cpp_v3/control_node.h"
@@ -25,13 +24,13 @@ namespace BT
  * __concurrently__, but not in separate threads!
  *
  * Even if this may look similar to ReactiveSequence,
- * this Control Node is the only one that may have
- * multiple children in the RUNNING state at the same time.
+ * this Control Node is the __only__ one that can have
+ * multiple children RUNNING at the same time.
  *
  * The Node is completed either when the THRESHOLD_SUCCESS
  * or THRESHOLD_FAILURE number is reached (both configured using ports).
  *
- * If any of the threahold is reached, and other childen are still running,
+ * If any of the threaholds is reached, and other children are still running,
  * they will be halted.
  *
  * Note that threshold indexes work as in Python:
@@ -56,7 +55,7 @@ class ParallelNode : public ControlNode
                               "number of childen which need to fail to trigger a FAILURE" ) };
     }
 
-    ~ParallelNode() = default;
+    ~ParallelNode() override = default;
 
     virtual void halt() override;
 
@@ -79,4 +78,3 @@ class ParallelNode : public ControlNode
 };
 
 }
-#endif   // PARALLEL_NODE_H
diff --git a/include/behaviortree_cpp_v3/controls/reactive_fallback.h b/include/behaviortree_cpp_v3/controls/reactive_fallback.h
@@ -1,4 +1,4 @@
-/* Copyright (C) 2020 Davide Faconti, Eurecat -  All Rights Reserved
+/* Copyright (C) 2020-2022 Davide Faconti, Eurecat -  All Rights Reserved
 *
 *   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
 *   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
@@ -10,8 +10,7 @@
 *   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
-#ifndef REACTIVE_FALLBACK_NODE_H
-#define REACTIVE_FALLBACK_NODE_H
+#pragma once
 
 #include "behaviortree_cpp_v3/control_node.h"
 
@@ -45,4 +44,3 @@ class ReactiveFallback : public ControlNode
 };
 
 }
-#endif   // REACTIVE_FALLBACK_NODE_H