From 6729fb3ac35f04c96a20a144cb21c2183dfece02 Mon Sep 17 00:00:00 2001
From: Devis Dal Moro <devis.dalmoro@eurecat.org>
Date: Mon, 31 Mar 2025 12:37:11 +0000
Subject: [PATCH 1/5] Set blackboard bug fix

Previous SetBlackboard action node did not update the sequence id, the timestamp when called on a pre-existing entry.
Moreover, it allowed to bypass type checking.
---
 .../actions/set_blackboard_node.h             |  2 +-
 tests/gtest_blackboard.cpp                    | 72 +++++++++++++++++++
 2 files changed, 73 insertions(+), 1 deletion(-)

diff --git a/include/behaviortree_cpp/actions/set_blackboard_node.h b/include/behaviortree_cpp/actions/set_blackboard_node.h
index 9882cea6f..6a9b7bbda 100644
--- a/include/behaviortree_cpp/actions/set_blackboard_node.h
+++ b/include/behaviortree_cpp/actions/set_blackboard_node.h
@@ -78,7 +78,7 @@ class SetBlackboardNode : public SyncActionNode
         config().blackboard->createEntry(output_key, src_entry->info);
         dst_entry = config().blackboard->getEntry(output_key);
       }
-      dst_entry->value = src_entry->value;
+      config().blackboard->set(output_key, src_entry->value);
     }
     else
     {
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index fcbc272a3..ad826cb10 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -654,3 +654,75 @@ TEST(BlackboardTest, TimestampedInterface)
   ASSERT_EQ(stamp_opt->seq, 2);
   ASSERT_GE(stamp_opt->time.count(), nsec_before);
 }
+
+TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+  <root BTCPP_format="4">
+    <BehaviorTree ID="MainTree">
+      <Sequence>
+        <Script code="other_point:=first_point" />
+        <Sleep msec="5" />
+        <SetBlackboard value="{second_point}" output_key="other_point" />
+      </Sequence>
+    </BehaviorTree>
+  </root> )";
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+  auto& blackboard = tree.subtrees.front()->blackboard;
+
+  const Point point1 = { 2, 2 };
+  const Point point2 = { 3, 3 };
+  blackboard->set("first_point", point1);
+  blackboard->set("second_point", point2);
+
+  tree.tickExactlyOnce();
+  const auto entry_ptr = blackboard->getEntry("other_point");
+  const auto ts1 = entry_ptr->stamp;
+  const auto seq_id1 = entry_ptr->sequence_id;
+  std::this_thread::sleep_for(std::chrono::milliseconds{5});
+  tree.tickExactlyOnce();
+  const auto ts2 = entry_ptr->stamp;
+  const auto seq_id2 = entry_ptr->sequence_id;
+  
+  ASSERT_GT(ts2.count(), ts1.count());
+  ASSERT_GT(seq_id2, seq_id1);
+}
+
+TEST(BlackboardTest, SetBlackboard_ChangeType)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+  <root BTCPP_format="4">
+    <BehaviorTree ID="MainTree">
+      <Sequence>
+        <SetBlackboard value="{first_point}" output_key="other_point" />
+        <Sleep msec="5" />
+        <SetBlackboard value="{random_str}" output_key="other_point" />
+      </Sequence>
+    </BehaviorTree>
+  </root> )";
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+  auto& blackboard = tree.subtrees.front()->blackboard;
+
+  const Point point = { 2, 7 };
+  blackboard->set("first_point", point);
+  blackboard->set("random_str", "Hello!");
+
+  // First tick should succeed
+  ASSERT_NO_THROW(tree.tickExactlyOnce());
+  const auto entry_ptr = blackboard->getEntry("other_point");
+  std::this_thread::sleep_for(std::chrono::milliseconds{5});
+  // Second tick should throw due to type mismatch
+  EXPECT_THROW({
+    tree.tickExactlyOnce();
+  }, BT::LogicError);
+  // EXPECT_EQ();
+
+}

From bfa14d726eb6c83a5d40eefbc04c880666e52489 Mon Sep 17 00:00:00 2001
From: root <you@example.com>
Date: Tue, 1 Apr 2025 06:41:51 +0000
Subject: [PATCH 2/5] Fix clang format

---
 tests/gtest_blackboard.cpp | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index ad826cb10..06bfccd8d 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -683,11 +683,11 @@ TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId)
   const auto entry_ptr = blackboard->getEntry("other_point");
   const auto ts1 = entry_ptr->stamp;
   const auto seq_id1 = entry_ptr->sequence_id;
-  std::this_thread::sleep_for(std::chrono::milliseconds{5});
+  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   tree.tickExactlyOnce();
   const auto ts2 = entry_ptr->stamp;
   const auto seq_id2 = entry_ptr->sequence_id;
-  
+
   ASSERT_GT(ts2.count(), ts1.count());
   ASSERT_GT(seq_id2, seq_id1);
 }
@@ -718,11 +718,8 @@ TEST(BlackboardTest, SetBlackboard_ChangeType)
   // First tick should succeed
   ASSERT_NO_THROW(tree.tickExactlyOnce());
   const auto entry_ptr = blackboard->getEntry("other_point");
-  std::this_thread::sleep_for(std::chrono::milliseconds{5});
+  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   // Second tick should throw due to type mismatch
-  EXPECT_THROW({
-    tree.tickExactlyOnce();
-  }, BT::LogicError);
+  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
   // EXPECT_EQ();
-
 }

From 60bc423425d2a16aef06532524548bdf5dc045d1 Mon Sep 17 00:00:00 2001
From: root <you@example.com>
Date: Tue, 1 Apr 2025 09:47:30 +0000
Subject: [PATCH 3/5] Fixed new test cases

---
 tests/gtest_blackboard.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index 06bfccd8d..35fa7dd9b 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -683,8 +683,7 @@ TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId)
   const auto entry_ptr = blackboard->getEntry("other_point");
   const auto ts1 = entry_ptr->stamp;
   const auto seq_id1 = entry_ptr->sequence_id;
-  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
-  tree.tickExactlyOnce();
+  tree.tickWhileRunning();
   const auto ts2 = entry_ptr->stamp;
   const auto seq_id2 = entry_ptr->sequence_id;
 
@@ -718,8 +717,7 @@ TEST(BlackboardTest, SetBlackboard_ChangeType)
   // First tick should succeed
   ASSERT_NO_THROW(tree.tickExactlyOnce());
   const auto entry_ptr = blackboard->getEntry("other_point");
-  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   // Second tick should throw due to type mismatch
-  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
+  EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
   // EXPECT_EQ();
 }

From c1e9f343318c6debaedf1e11bfc15aa05676902c Mon Sep 17 00:00:00 2001
From: root <you@example.com>
Date: Mon, 7 Apr 2025 14:33:11 +0000
Subject: [PATCH 4/5] Types issues when port remapping in SetBlackboard

Made sure to handle possible type inconsistencies which might arise when using port remapping with subtrees.
---
 .../actions/set_blackboard_node.h             |  33 ++++-
 tests/gtest_blackboard.cpp                    | 139 +++++++++++++++++-
 2 files changed, 165 insertions(+), 7 deletions(-)

diff --git a/include/behaviortree_cpp/actions/set_blackboard_node.h b/include/behaviortree_cpp/actions/set_blackboard_node.h
index 6a9b7bbda..1a240c3db 100644
--- a/include/behaviortree_cpp/actions/set_blackboard_node.h
+++ b/include/behaviortree_cpp/actions/set_blackboard_node.h
@@ -61,13 +61,16 @@ class SetBlackboardNode : public SyncActionNode
     const std::string value_str = config().input_ports.at("value");
 
     StringView stripped_key;
+    BT::Any out_value;
+
+    std::shared_ptr<Blackboard::Entry> dst_entry =
+        config().blackboard->getEntry(output_key);
+
     if(isBlackboardPointer(value_str, &stripped_key))
     {
       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)
       {
@@ -78,13 +81,35 @@ class SetBlackboardNode : public SyncActionNode
         config().blackboard->createEntry(output_key, src_entry->info);
         dst_entry = config().blackboard->getEntry(output_key);
       }
-      config().blackboard->set(output_key, src_entry->value);
+
+      out_value = src_entry->value;
     }
     else
     {
-      config().blackboard->set(output_key, value_str);
+      out_value = BT::Any(value_str);
+    }
+
+    if(out_value.empty())
+      return NodeStatus::FAILURE;
+
+    // avoid type issues when port is remapped: current implementation of the set might be a little bit problematic for initialized on the fly values
+    // this still does not attack math issues
+    if(dst_entry && dst_entry->info.type() != typeid(std::string) && out_value.isString())
+    {
+      try
+      {
+        out_value = dst_entry->info.parseString(out_value.cast<std::string>());
+      }
+      catch(const std::exception& e)
+      {
+        throw LogicError("Can't convert string [", out_value.cast<std::string>(),
+                         "] to type [", BT::demangle(dst_entry->info.type()),
+                         "]: ", e.what());
+      }
     }
 
+    config().blackboard->set(output_key, out_value);
+
     return NodeStatus::SUCCESS;
   }
 };
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index 35fa7dd9b..6ab534e9a 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -458,6 +458,29 @@ struct Point
   double y;
 };
 
+// Template specialization to converts a string to Point.
+namespace BT
+{
+template <>
+[[nodiscard]] Point convertFromString(StringView str)
+{
+  // We expect real numbers separated by semicolons
+  auto parts = splitString(str, ';');
+  if(parts.size() != 2)
+  {
+    throw RuntimeError("invalid input)");
+  }
+  else
+  {
+    Point output{ 0.0, 0.0 };
+    output.x = convertFromString<double>(parts[0]);
+    output.y = convertFromString<double>(parts[1]);
+    // std::cout << "Building a position 2d object " << output.x << "; " << output.y << "\n" << std::flush;
+    return output;
+  }
+}
+}  // end namespace BT
+
 TEST(BlackboardTest, SetBlackboard_Issue725)
 {
   BT::BehaviorTreeFactory factory;
@@ -691,7 +714,7 @@ TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId)
   ASSERT_GT(seq_id2, seq_id1);
 }
 
-TEST(BlackboardTest, SetBlackboard_ChangeType)
+TEST(BlackboardTest, SetBlackboard_ChangeType1)
 {
   BT::BehaviorTreeFactory factory;
 
@@ -717,7 +740,117 @@ TEST(BlackboardTest, SetBlackboard_ChangeType)
   // First tick should succeed
   ASSERT_NO_THROW(tree.tickExactlyOnce());
   const auto entry_ptr = blackboard->getEntry("other_point");
+  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   // Second tick should throw due to type mismatch
-  EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
-  // EXPECT_EQ();
+  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
+}
+
+TEST(BlackboardTest, SetBlackboard_ChangeType2)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+  <root BTCPP_format="4">
+    <BehaviorTree ID="MainTree">
+      <Sequence>
+        <SetBlackboard value="{first_point}" output_key="other_point" />
+        <Sleep msec="5" />
+        <SetBlackboard value="{random_num}" output_key="other_point" />
+      </Sequence>
+    </BehaviorTree>
+  </root> )";
+
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+  auto& blackboard = tree.subtrees.front()->blackboard;
+
+  const Point point = { 2, 7 };
+  blackboard->set("first_point", point);
+  blackboard->set("random_num", 57);
+
+  // First tick should succeed
+  ASSERT_NO_THROW(tree.tickExactlyOnce());
+  const auto entry_ptr = blackboard->getEntry("other_point");
+  std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
+  // Second tick should throw due to type mismatch
+  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
+}
+
+// Simple Action that updates an instance of Point in the blackboard
+class UpdatePosition : public BT::SyncActionNode
+{
+public:
+  UpdatePosition(const std::string& name, const BT::NodeConfig& config)
+    : BT::SyncActionNode(name, config)
+  {}
+
+  BT::NodeStatus tick() override
+  {
+    const auto in_pos = getInput<Point>("pos_in");
+    if(!in_pos.has_value())
+      return BT::NodeStatus::FAILURE;
+    Point _pos = in_pos.value();
+    _pos.x += getInput<double>("x").value_or(0.0);
+    _pos.y += getInput<double>("y").value_or(0.0);
+    setOutput("pos_out", _pos);
+    return BT::NodeStatus::SUCCESS;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return { BT::InputPort<Point>("pos_in", { 0.0, 0.0 }, "Initial position"),
+             BT::InputPort<double>("x"), BT::InputPort<double>("y"),
+             BT::OutputPort<Point>("pos_out") };
+  }
+
+private:
+};
+
+TEST(BlackboardTest, SetBlackboard_WithPortRemapping)
+{
+  BT::BehaviorTreeFactory factory;
+
+  const std::string xml_text = R"(
+    <?xml version="1.0"?>
+    <root BTCPP_format="4" main_tree_to_execute="MainTree">
+      <BehaviorTree ID="MainTree">
+          <Sequence>
+              <SetBlackboard output_key="pos" value="0.0;0.0" />
+              <Repeat num_cycles="3">
+                  <Sequence>
+                      <UpdatePosition pos_in="{pos}" x="0.1" y="0.2" pos_out="{pos}"/>
+                      <SubTree ID="UpdPosPlus" _autoremap="true" new_pos="2.2;2.4" />
+                      <Sleep msec="125"/>
+                      <SetBlackboard output_key="pos" value="22.0;22.0" />
+                  </Sequence>
+              </Repeat>
+          </Sequence>
+      </BehaviorTree>
+      <BehaviorTree ID="UpdPosPlus">
+          <Sequence>
+              <SetBlackboard output_key="pos" value="3.0;5.0" />
+              <SetBlackboard output_key="pos" value="{new_pos}" />
+          </Sequence>
+      </BehaviorTree>
+    </root>
+  )";
+
+  factory.registerNodeType<UpdatePosition>("UpdatePosition");
+  factory.registerBehaviorTreeFromText(xml_text);
+  auto tree = factory.createTree("MainTree");
+  auto& blackboard = tree.subtrees.front()->blackboard;
+
+  // First tick should succeed and update the value within the subtree
+  ASSERT_NO_THROW(tree.tickExactlyOnce());
+
+  const auto entry_ptr = blackboard->getEntry("pos");
+  ASSERT_EQ(entry_ptr->value.type(), typeid(Point));
+
+  const auto x = entry_ptr->value.cast<Point>().x;
+  const auto y = entry_ptr->value.cast<Point>().y;
+  ASSERT_EQ(x, 2.2);
+  ASSERT_EQ(y, 2.4);
+
+  // Tick till the end with no crashes
+  ASSERT_NO_THROW(tree.tickWhileRunning(););
 }

From dbda53e8982822467d472330e77b50f01dba3f4f Mon Sep 17 00:00:00 2001
From: root <you@example.com>
Date: Mon, 7 Apr 2025 14:55:05 +0000
Subject: [PATCH 5/5] Make sure of logic error gets thrown in SetBlackboard
 ChangeType tests

---
 tests/gtest_blackboard.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index 6ab534e9a..35be37dab 100644
--- a/tests/gtest_blackboard.cpp
+++ b/tests/gtest_blackboard.cpp
@@ -742,7 +742,7 @@ TEST(BlackboardTest, SetBlackboard_ChangeType1)
   const auto entry_ptr = blackboard->getEntry("other_point");
   std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   // Second tick should throw due to type mismatch
-  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
+  EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
 }
 
 TEST(BlackboardTest, SetBlackboard_ChangeType2)
@@ -773,7 +773,7 @@ TEST(BlackboardTest, SetBlackboard_ChangeType2)
   const auto entry_ptr = blackboard->getEntry("other_point");
   std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
   // Second tick should throw due to type mismatch
-  EXPECT_THROW({ tree.tickExactlyOnce(); }, BT::LogicError);
+  EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
 }
 
 // Simple Action that updates an instance of Point in the blackboard