diff --git a/include/behaviortree_cpp/actions/set_blackboard_node.h b/include/behaviortree_cpp/actions/set_blackboard_node.h
index 9882cea6f..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);
}
- 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;
}
};
diff --git a/tests/gtest_blackboard.cpp b/tests/gtest_blackboard.cpp
index fcbc272a3..35be37dab 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;
@@ -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(););
+}