fix issue BehaviorTree#725 : SetBlackboard can copy entries

facontidavide · facontidavide · commit a53a95f1f938 · 2023-12-19T11:18:05.000+01:00
diff --git a/include/behaviortree_cpp/actions/set_blackboard_node.h b/include/behaviortree_cpp/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
 {
@@ -46,17 +52,36 @@ class SetBlackboard : public SyncActionNode
 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");
+
+    StringView stripped_key;
+    if(isBlackboardPointer(value_str, &stripped_key))
     {
-      throw RuntimeError("missing port [value]");
-    }
+      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);
 
-    config().blackboard->set(static_cast<std::string>(key), value);
+      if(!src_entry)
+      {
+        throw RuntimeError("Can't find the port referred by [value]");
+      }
+      if(!dst_entry)
+      {
+        config().blackboard->createEntry(output_key, src_entry->info);
+        dst_entry = config().blackboard->getEntry(output_key);
+      }
+      dst_entry->value = src_entry->value;
+    }
+    else
+    {
+      config().blackboard->set(output_key, value_str);
+    }
 
     return NodeStatus::SUCCESS;
   }
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
@@ -461,29 +461,37 @@ TEST(BlackboardTest, IssueSetBlackboard)
   ASSERT_EQ(42, tree.rootBlackboard()->get<int>("value"));
 }
 
+struct Point {
+  double x;
+  double y;
+};
+
 TEST(BlackboardTest, SetBlackboard_Issue725)
 {
   BT::BehaviorTreeFactory factory;
 
   const std::string xml_text = R"(
-  <root BTCPP_format="4" >
+  <root BTCPP_format="4">
     <BehaviorTree ID="MainTree">
-      <Sequence>
-        <SetBlackboard value="hello world" output_key="value" />
-        <SetBlackboard value="{value}" output_key="other_value" />
-        <SaySomething message="{other_value}" />
-      </Sequence>
+      <SetBlackboard value="{first_point}" output_key="other_point" />
     </BehaviorTree>
   </root> )";
 
   factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
   factory.registerBehaviorTreeFromText(xml_text);
   auto tree = factory.createTree("MainTree");
-  const auto status = tree.tickWhileRunning();
+  auto& blackboard = tree.subtrees.front()->blackboard;
+
+  const Point point = {2,7};
+  blackboard->set("first_point", point);
+
+  const auto status = tree.tickOnce();
+
+  Point other_point = blackboard->get<Point>("other_point");
 
   ASSERT_EQ(status, BT::NodeStatus::SUCCESS);
-  ASSERT_EQ("hello world", tree.rootBlackboard()->get<std::string>("value"));
-  ASSERT_EQ("hello world", tree.rootBlackboard()->get<std::string>("other_value"));
+  ASSERT_EQ(other_point.x, point.x);
+  ASSERT_EQ(other_point.y, point.y);
 }
 
 
