fix thread safety

Author: facontidavide

docs/images/SequenceStar.svg

@@ -17,6 +17,8 @@
 #include <atomic>
 #include <thread>
 #include <future>
+#include <mutex>
+
 #include "leaf_node.h"
 
 namespace BT
@@ -135,6 +137,7 @@ class AsyncActionNode : public ActionNodeBase
     std::exception_ptr exptr_;
     std::atomic_bool halt_requested_;
     std::future<NodeStatus> thread_handle_;
+    std::mutex m_;
 };
 
 /**

include/behaviortree_cpp_v3/action_node.h

@@ -117,6 +117,7 @@ class Blackboard
     void set(const std::string& key, const T& value)
     {
         std::unique_lock<std::mutex> lock(mutex_);
+        std::unique_lock<std::mutex> lock_entry(entry_mutex_);
         auto it = storage_.find(key);
 
         if( auto parent = parent_bb_.lock())
@@ -130,10 +131,10 @@ class Blackboard
                     auto parent_info = parent->portInfo(remapped_key);
                     if( parent_info )
                     {
-                        storage_.insert( {key, Entry( *parent_info ) } );
+                        storage_.emplace( key, Entry( *parent_info ) );
                     }
                     else{
-                        storage_.insert( {key, Entry( PortInfo() ) } );
+                        storage_.emplace( key, Entry( PortInfo() ) );
                     }
                 }
                 parent->set( remapped_key, value );
@@ -195,10 +196,18 @@ class Blackboard
         storage_.clear();
         internal_to_external_.clear();
     }
+
+    // Lock this mutex before using get() and getAny() and unlock it while you have
+    // done using the value.
+    std::mutex& entryMutex()
+    {
+      return entry_mutex_;
+    }
 
   private:
 
     struct Entry{
+
         Any value;
         const PortInfo port_info;
 
@@ -213,6 +222,7 @@ class Blackboard
     };
 
     mutable std::mutex mutex_;
+    mutable std::mutex entry_mutex_;
     std::unordered_map<std::string, Entry> storage_;
     std::weak_ptr<Blackboard> parent_bb_;
     std::unordered_map<std::string,std::string> internal_to_external_;

include/behaviortree_cpp_v3/blackboard.h

@@ -215,6 +215,7 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const
                                            "but BB is invalid");
         }
 
+        std::unique_lock<std::mutex> entry_lock( config_.blackboard->entryMutex() );
         const Any* val = config_.blackboard->getAny(static_cast<std::string>(remapped_key));
         if (val && val->empty() == false)
         {

include/behaviortree_cpp_v3/tree_node.h

@@ -18,6 +18,7 @@
 
   <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
   <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+  <depend condition="$ROS_VERSION == 2">ament_index_cpp</depend>
 
   <depend>boost</depend>
   <depend>libzmq3-dev</depend>

package.xml

@@ -167,6 +167,7 @@ void StatefulActionNode::halt()
 
 NodeStatus BT::AsyncActionNode::executeTick()
 {
+    using lock_type = std::unique_lock<std::mutex>;
     //send signal to other thread.
     // The other thread is in charge for changing the status
     if (status() == NodeStatus::IDLE)
@@ -182,16 +183,23 @@ NodeStatus BT::AsyncActionNode::executeTick()
             {
                 std::cerr << "\nUncaught exception from the method tick(): ["
                           << registrationName() << "/" << name() << "]\n" << std::endl;
+                // Set the exception pointer and the status atomically.
+                lock_type l(m_);
                 exptr_ = std::current_exception();
-                thread_handle_.wait();
+                setStatus(BT::NodeStatus::IDLE);
             }
             return status();
         });
     }
 
+    lock_type l(m_);
     if( exptr_ )
     {
-        std::rethrow_exception(exptr_);
+        // The official interface of std::exception_ptr does not define any move
+        // semantics. Thus, we copy and reset exptr_ manually.
+        const auto exptr_copy = exptr_;
+        exptr_ = nullptr;
+        std::rethrow_exception(exptr_copy);
     }
     return status();
 }

src/action_node.cpp

@@ -18,7 +18,7 @@ void Blackboard::setPortInfo(std::string key, const PortInfo& info)
     auto it = storage_.find(key);
     if( it == storage_.end() )
     {
-        storage_.insert( { std::move(key), Entry(info) } );
+        storage_.emplace( key, Entry(info) );
     }
     else{
         auto old_type = it->second.port_info.type();