improve Any::castPtr and add example

facontidavide · facontidavide · commit af243ad576a8 · 2024-01-22T14:02:55.000+01:00
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -37,6 +37,7 @@ CompileExample("ex01_wrap_legacy")
 CompileExample("ex02_runtime_ports")
 CompileExample("ex04_waypoints")
 CompileExample("ex05_subtree_model")
+CompileExample("ex06_access_by_ptr")
 
 CompileExample("t13_plugin_executor")
 
diff --git a/examples/ex04_waypoints.cpp b/examples/ex04_waypoints.cpp
@@ -1,7 +1,7 @@
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/decorators/loop_node.h"
 #include "behaviortree_cpp/loggers/bt_cout_logger.h"
-#include <list>
+#include <deque>
 
 using namespace BT;
 
diff --git a/examples/ex06_access_by_ptr.cpp b/examples/ex06_access_by_ptr.cpp
@@ -0,0 +1,77 @@
+#include "behaviortree_cpp/bt_factory.h"
+
+using namespace BT;
+
+/**
+ * @brief Show how to access an entry in the blackboard by pointer.
+ * This approach is more verbose, but thread-safe
+ *
+ */
+class PushIntoVector : public SyncActionNode
+{
+public:
+  PushIntoVector(const std::string& name, const NodeConfig& config) :
+    SyncActionNode(name, config)
+  {}
+
+  NodeStatus tick() override
+  {
+    const int number = getInput<int>("value").value();
+    if(auto any_ptr = getLockedPortContent("vector"))
+    {
+      // inside this scope, any_ptr provides a mutex-protected,
+      // thread-safe way to access an element inside the blackboard.
+      if(auto vect_ptr = any_ptr->castPtr<std::vector<int>>())
+      {
+        vect_ptr->push_back(number);
+        std::cout << "Value ["<< number <<"] pushed into the vector. New size: "
+                  << vect_ptr->size() << "\n";
+      }
+      else {
+        // This happens when the entry of the blackboard is empty or if
+        // we tried to cast to the wrong type.
+        // Let's insert a new vector<int> with a single value
+        std::vector<int> vect = {number};
+        any_ptr.assign(vect);
+        std::cout << "We created a new vector, containing value ["<< number <<"]\n";
+      }
+      return NodeStatus::SUCCESS;
+    }
+    else {
+      return NodeStatus::FAILURE;
+    }
+  }
+
+  static PortsList providedPorts()
+  {
+    return {BT::BidirectionalPort<std::vector<int>>("vector"),
+            BT::InputPort<int>("value")};
+  }
+};
+
+//--------------------------------------------------------------
+
+// clang-format off
+static const char* xml_tree = R"(
+  <root BTCPP_format="4" >
+    <BehaviorTree ID="TreeA">
+      <Sequence>
+        <PushIntoVector vector="{vect}" value="3"/>
+        <PushIntoVector vector="{vect}" value="5"/>
+        <PushIntoVector vector="{vect}" value="7"/>
+      </Sequence>
+    </BehaviorTree>
+ </root>
+ )";
+
+// clang-format on
+
+int main()
+{
+  BehaviorTreeFactory factory;
+  factory.registerNodeType<PushIntoVector>("PushIntoVector");
+
+  auto tree = factory.createTreeFromText(xml_tree);
+  tree.tickWhileRunning();
+  return 0;
+}
diff --git a/include/behaviortree_cpp/utils/locked_reference.hpp b/include/behaviortree_cpp/utils/locked_reference.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <mutex>
+#include "behaviortree_cpp/utils/safe_any.hpp"
 
 namespace BT
 {
@@ -9,15 +10,15 @@ namespace BT
  * and a mutex that protects the read/write access to that object.
  *
  * As long as the object remains in scope, the mutex is locked, therefore
- * you must destroy this object as soon as the pointer was used.
+ * you must destroy this instance as soon as the pointer was used.
  */
 template <typename T>
 class LockedPtr {
   public:
 
   LockedPtr() = default;
 
-  LockedPtr(const T* obj, std::mutex* obj_mutex):
+  LockedPtr(T* obj, std::mutex* obj_mutex):
         ref_(obj), mutex_(obj_mutex) {
     mutex_->lock();
   }
@@ -65,8 +66,36 @@ class LockedPtr {
     return ref_;
   }
 
+  const T* operator->() const{
+    return ref_;
+  }
+
+  T* operator->() {
+    return ref_;
+  }
+
+  template <typename OtherT>
+  void assign(const OtherT& other)
+  {
+    if(ref_ == nullptr)
+    {
+      throw std::runtime_error("Empty LockedPtr reference");
+    }
+    else if constexpr(std::is_same_v<T, OtherT>)
+    {
+      *ref_ = other;
+    }
+    else if constexpr( std::is_same_v<BT::Any, OtherT>)
+    {
+      other->copyInto(*ref_);
+    }
+    else {
+      *ref_ = T(other);
+    }
+  }
+
   private:
-  const T* ref_ = nullptr;
+  T* ref_ = nullptr;
   std::mutex* mutex_ = nullptr;
 };
 
diff --git a/include/behaviortree_cpp/utils/safe_any.hpp b/include/behaviortree_cpp/utils/safe_any.hpp
@@ -160,15 +160,21 @@ class Any
     }
   }
 
-  // Method to access the value by pointer
+  // Method to access the value by pointer.
+  // It will return nullptr, if the user try to cast it to a
+  // wrong type or if Any was empty.
   template <typename T>
-  [[nodiscard]] T* castPtr() const {
+  [[nodiscard]] T* castPtr() {
 
-    if( _any.empty() )
-    {
-      return nullptr;
-    }
-    return linb::any_cast<T>(&_any);
+    static_assert(!std::is_same_v<T, float>,
+                  "The value has been casted internally to [double]. "
+                  "Use that instead");
+    static_assert(!SafeAny::details::is_integer<T>() ||
+                  std::is_same_v<T, uint64_t>,
+                  "The value has been casted internally to [int64_t]. "
+                  "Use that instead");
+
+    return _any.empty() ? nullptr : linb::any_cast<T>(&_any);
   }
 
   // This is the original type
diff --git a/src/loggers/groot2_publisher.cpp b/src/loggers/groot2_publisher.cpp
@@ -1,4 +1,3 @@
-#include <future>
 #include "behaviortree_cpp/json_export.h"
 #include "behaviortree_cpp/loggers/groot2_publisher.h"
 #include "behaviortree_cpp/loggers/groot2_protocol.h"

Original file line number	Diff line number	Diff line change
`@@ -1,4 +1,3 @@`
`1`		`-#include <future>`
`2`	`1`	`#include "behaviortree_cpp/json_export.h"`
`3`	`2`	`#include "behaviortree_cpp/loggers/groot2_publisher.h"`
`4`	`3`	`#include "behaviortree_cpp/loggers/groot2_protocol.h"`