fix issue #725 : SetBlackboard can copy entries

include/behaviortree_cpp_v3/actions/set_blackboard_node.h

@@ -26,6 +26,12 @@ namespace BT
  *  <SetBlackboard value="42" output_key="the_answer" />
  *
  * Will store the string "42" in the entry with key "the_answer".
+ *
+ * Alternatively, you can use it to copy one port inside another port:
+ *
+ * <SetBlackboard value="{src_port}" output_key="dst_port" />
+ *
+ * This will copy the type and content of {src_port} into {dst_port}
  */
 class SetBlackboard : public SyncActionNode
 {
@@ -38,26 +44,45 @@ class SetBlackboard : public SyncActionNode
 
   static PortsList providedPorts()
   {
-    return {InputPort("value", "Value represented as a string. convertFromString must be "
-                               "implemented."),
+    return {InputPort("value", "Value to be written int othe output_key"),
             BidirectionalPort("output_key", "Name of the blackboard entry where the "
-                                            "value "
-                                            "should be written")};
+                                            "value should be written")};
   }
 
 private:
   virtual BT::NodeStatus tick() override
   {
-    std::string key, value;
-    if (!getInput("output_key", key))
+    std::string output_key;
+    if (!getInput("output_key", output_key))
     {
       throw RuntimeError("missing port [output_key]");
     }
-    if (!getInput("value", value))
+
+    const std::string value_str = config().input_ports.at("value");
+
+    if(isBlackboardPointer(value_str))
     {
-      throw RuntimeError("missing port [value]");
+      StringView stripped_key = stripBlackboardPointer(value_str);
+      const auto input_key = std::string(stripped_key);
+      std::shared_ptr<Blackboard::Entry> src_entry = config().blackboard->getEntry(input_key);
+      std::shared_ptr<Blackboard::Entry> dst_entry = config().blackboard->getEntry(output_key);
+
+      if(!src_entry)
+      {
+        throw RuntimeError("Can't find the port referred by [value]");
+      }
+      if(!dst_entry)
+      {
+        config().blackboard->createEntry(output_key, src_entry->port_info);
+        dst_entry = config().blackboard->getEntry(output_key);
+      }
+      dst_entry->value = src_entry->value;
+    }
+    else
+    {
+      config().blackboard->set(output_key, value_str);
     }
-    setOutput("output_key", value);
+
     return NodeStatus::SUCCESS;
   }
 };

include/behaviortree_cpp_v3/blackboard.h

@@ -178,7 +178,6 @@ class Blackboard
     createEntryImpl(key, info);
   }
 
-private:
   struct Entry
   {
     Any value;
@@ -188,10 +187,18 @@ class Blackboard
     {}
 
     Entry(Any&& other_any, const PortInfo& info) :
-      value(std::move(other_any)), port_info(info)
+        value(std::move(other_any)), port_info(info)
     {}
   };
 
+  std::shared_ptr<Entry> getEntry(const std::string& key) const
+  {
+    auto it = storage_.find(key);
+    return it == storage_.end() ? std::shared_ptr<Entry>() : it->second;
+  }
+
+private:
+
   std::shared_ptr<Entry> createEntryImpl(const std::string& key, const PortInfo& info);
 
   mutable std::mutex mutex_;

tests/gtest_blackboard.cpp

@@ -1,4 +1,4 @@
-/* Copyright (C) 2018-2019 Davide Faconti, Eurecat - All Rights Reserved
+/* Copyright (C) 2018-2023 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,12 +11,10 @@
 */
 
 #include <gtest/gtest.h>
-#include "action_test_node.h"
-#include "condition_test_node.h"
-#include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behaviortree_cpp_v3/blackboard.h"
-#include "behaviortree_cpp_v3/xml_parsing.h"
+
+#include "../sample_nodes/dummy_nodes.h"
 
 using namespace BT;
 
@@ -295,3 +293,37 @@ TEST(BlackboardTest, CheckTypeSafety)
   is = std::is_constructible<BT::StringView, std::string>::value;
   ASSERT_TRUE(is);
 }
+
+struct Point {
+  double x;
+  double y;
+};
+
+TEST(BlackboardTest, SetBlackboard_Issue725)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+  <root main_tree_to_execute = "MainTree"  >
+    <BehaviorTree ID="MainTree">
+      <SetBlackboard value="{first_point}" output_key="other_point" />
+    </BehaviorTree>
+  </root> )";
+
+  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+  auto& blackboard = tree.blackboard_stack.front();
+
+  const Point point = {2,7};
+  blackboard->set("first_point", point);
+
+  const auto status = tree.tickRoot();
+
+  Point other_point = blackboard->get<Point>("other_point");
+
+  ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
+  ASSERT_EQ(other_point.x, point.x);
+  ASSERT_EQ(other_point.y, point.y);
+}
+