Bug fix/set blackboard

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);
       }
-      dst_entry->value = 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;
   }
 };

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;
@@ -654,3 +677,180 @@ 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;
+  tree.tickWhileRunning();
+  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_ChangeType1)
+{
+  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.tickWhileRunning(); }, 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.tickWhileRunning(); }, 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(););
+}