Skip to content

Commit 3651a24

Browse files
devis12devis12
authored
Bug fix/set blackboard (#955)
1 parent 14589e5 commit 3651a24

File tree

2 files changed

+229
-4
lines changed

2 files changed

+229
-4
lines changed

‎include/behaviortree_cpp/actions/set_blackboard_node.h‎

Lines changed: 29 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -61,13 +61,16 @@ class SetBlackboardNode : public SyncActionNode
6161
const std::string value_str = config().input_ports.at("value");
6262

6363
StringView stripped_key;
64+
BT::Any out_value;
65+
66+
std::shared_ptr<Blackboard::Entry> dst_entry =
67+
config().blackboard->getEntry(output_key);
68+
6469
if(isBlackboardPointer(value_str, &stripped_key))
6570
{
6671
const auto input_key = std::string(stripped_key);
6772
std::shared_ptr<Blackboard::Entry> src_entry =
6873
config().blackboard->getEntry(input_key);
69-
std::shared_ptr<Blackboard::Entry> dst_entry =
70-
config().blackboard->getEntry(output_key);
7174

7275
if(!src_entry)
7376
{
@@ -78,13 +81,35 @@ class SetBlackboardNode : public SyncActionNode
7881
config().blackboard->createEntry(output_key, src_entry->info);
7982
dst_entry = config().blackboard->getEntry(output_key);
8083
}
81-
dst_entry->value = src_entry->value;
84+
85+
out_value = src_entry->value;
8286
}
8387
else
8488
{
85-
config().blackboard->set(output_key, value_str);
89+
out_value = BT::Any(value_str);
90+
}
91+
92+
if(out_value.empty())
93+
return NodeStatus::FAILURE;
94+
95+
// avoid type issues when port is remapped: current implementation of the set might be a little bit problematic for initialized on the fly values
96+
// this still does not attack math issues
97+
if(dst_entry && dst_entry->info.type() != typeid(std::string) && out_value.isString())
98+
{
99+
try
100+
{
101+
out_value = dst_entry->info.parseString(out_value.cast<std::string>());
102+
}
103+
catch(const std::exception& e)
104+
{
105+
throw LogicError("Can't convert string [", out_value.cast<std::string>(),
106+
"] to type [", BT::demangle(dst_entry->info.type()),
107+
"]: ", e.what());
108+
}
86109
}
87110

111+
config().blackboard->set(output_key, out_value);
112+
88113
return NodeStatus::SUCCESS;
89114
}
90115
};

‎tests/gtest_blackboard.cpp‎

Lines changed: 200 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -458,6 +458,29 @@ struct Point
458458
double y;
459459
};
460460

461+
// Template specialization to converts a string to Point.
462+
namespace BT
463+
{
464+
template <>
465+
[[nodiscard]] Point convertFromString(StringView str)
466+
{
467+
// We expect real numbers separated by semicolons
468+
auto parts = splitString(str, ';');
469+
if(parts.size() != 2)
470+
{
471+
throw RuntimeError("invalid input)");
472+
}
473+
else
474+
{
475+
Point output{ 0.0, 0.0 };
476+
output.x = convertFromString<double>(parts[0]);
477+
output.y = convertFromString<double>(parts[1]);
478+
// std::cout << "Building a position 2d object " << output.x << "; " << output.y << "\n" << std::flush;
479+
return output;
480+
}
481+
}
482+
} // end namespace BT
483+
461484
TEST(BlackboardTest, SetBlackboard_Issue725)
462485
{
463486
BT::BehaviorTreeFactory factory;
@@ -654,3 +677,180 @@ TEST(BlackboardTest, TimestampedInterface)
654677
ASSERT_EQ(stamp_opt->seq, 2);
655678
ASSERT_GE(stamp_opt->time.count(), nsec_before);
656679
}
680+
681+
TEST(BlackboardTest, SetBlackboard_Upd_Ts_SeqId)
682+
{
683+
BT::BehaviorTreeFactory factory;
684+
685+
const std::string xml_text = R"(
686+
<root BTCPP_format="4">
687+
<BehaviorTree ID="MainTree">
688+
<Sequence>
689+
<Script code="other_point:=first_point" />
690+
<Sleep msec="5" />
691+
<SetBlackboard value="{second_point}" output_key="other_point" />
692+
</Sequence>
693+
</BehaviorTree>
694+
</root> )";
695+
696+
factory.registerBehaviorTreeFromText(xml_text);
697+
auto tree = factory.createTree("MainTree");
698+
auto& blackboard = tree.subtrees.front()->blackboard;
699+
700+
const Point point1 = { 2, 2 };
701+
const Point point2 = { 3, 3 };
702+
blackboard->set("first_point", point1);
703+
blackboard->set("second_point", point2);
704+
705+
tree.tickExactlyOnce();
706+
const auto entry_ptr = blackboard->getEntry("other_point");
707+
const auto ts1 = entry_ptr->stamp;
708+
const auto seq_id1 = entry_ptr->sequence_id;
709+
tree.tickWhileRunning();
710+
const auto ts2 = entry_ptr->stamp;
711+
const auto seq_id2 = entry_ptr->sequence_id;
712+
713+
ASSERT_GT(ts2.count(), ts1.count());
714+
ASSERT_GT(seq_id2, seq_id1);
715+
}
716+
717+
TEST(BlackboardTest, SetBlackboard_ChangeType1)
718+
{
719+
BT::BehaviorTreeFactory factory;
720+
721+
const std::string xml_text = R"(
722+
<root BTCPP_format="4">
723+
<BehaviorTree ID="MainTree">
724+
<Sequence>
725+
<SetBlackboard value="{first_point}" output_key="other_point" />
726+
<Sleep msec="5" />
727+
<SetBlackboard value="{random_str}" output_key="other_point" />
728+
</Sequence>
729+
</BehaviorTree>
730+
</root> )";
731+
732+
factory.registerBehaviorTreeFromText(xml_text);
733+
auto tree = factory.createTree("MainTree");
734+
auto& blackboard = tree.subtrees.front()->blackboard;
735+
736+
const Point point = { 2, 7 };
737+
blackboard->set("first_point", point);
738+
blackboard->set("random_str", "Hello!");
739+
740+
// First tick should succeed
741+
ASSERT_NO_THROW(tree.tickExactlyOnce());
742+
const auto entry_ptr = blackboard->getEntry("other_point");
743+
std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
744+
// Second tick should throw due to type mismatch
745+
EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
746+
}
747+
748+
TEST(BlackboardTest, SetBlackboard_ChangeType2)
749+
{
750+
BT::BehaviorTreeFactory factory;
751+
752+
const std::string xml_text = R"(
753+
<root BTCPP_format="4">
754+
<BehaviorTree ID="MainTree">
755+
<Sequence>
756+
<SetBlackboard value="{first_point}" output_key="other_point" />
757+
<Sleep msec="5" />
758+
<SetBlackboard value="{random_num}" output_key="other_point" />
759+
</Sequence>
760+
</BehaviorTree>
761+
</root> )";
762+
763+
factory.registerBehaviorTreeFromText(xml_text);
764+
auto tree = factory.createTree("MainTree");
765+
auto& blackboard = tree.subtrees.front()->blackboard;
766+
767+
const Point point = { 2, 7 };
768+
blackboard->set("first_point", point);
769+
blackboard->set("random_num", 57);
770+
771+
// First tick should succeed
772+
ASSERT_NO_THROW(tree.tickExactlyOnce());
773+
const auto entry_ptr = blackboard->getEntry("other_point");
774+
std::this_thread::sleep_for(std::chrono::milliseconds{ 5 });
775+
// Second tick should throw due to type mismatch
776+
EXPECT_THROW({ tree.tickWhileRunning(); }, BT::LogicError);
777+
}
778+
779+
// Simple Action that updates an instance of Point in the blackboard
780+
class UpdatePosition : public BT::SyncActionNode
781+
{
782+
public:
783+
UpdatePosition(const std::string& name, const BT::NodeConfig& config)
784+
: BT::SyncActionNode(name, config)
785+
{}
786+
787+
BT::NodeStatus tick() override
788+
{
789+
const auto in_pos = getInput<Point>("pos_in");
790+
if(!in_pos.has_value())
791+
return BT::NodeStatus::FAILURE;
792+
Point _pos = in_pos.value();
793+
_pos.x += getInput<double>("x").value_or(0.0);
794+
_pos.y += getInput<double>("y").value_or(0.0);
795+
setOutput("pos_out", _pos);
796+
return BT::NodeStatus::SUCCESS;
797+
}
798+
799+
static BT::PortsList providedPorts()
800+
{
801+
return { BT::InputPort<Point>("pos_in", { 0.0, 0.0 }, "Initial position"),
802+
BT::InputPort<double>("x"), BT::InputPort<double>("y"),
803+
BT::OutputPort<Point>("pos_out") };
804+
}
805+
806+
private:
807+
};
808+
809+
TEST(BlackboardTest, SetBlackboard_WithPortRemapping)
810+
{
811+
BT::BehaviorTreeFactory factory;
812+
813+
const std::string xml_text = R"(
814+
<?xml version="1.0"?>
815+
<root BTCPP_format="4" main_tree_to_execute="MainTree">
816+
<BehaviorTree ID="MainTree">
817+
<Sequence>
818+
<SetBlackboard output_key="pos" value="0.0;0.0" />
819+
<Repeat num_cycles="3">
820+
<Sequence>
821+
<UpdatePosition pos_in="{pos}" x="0.1" y="0.2" pos_out="{pos}"/>
822+
<SubTree ID="UpdPosPlus" _autoremap="true" new_pos="2.2;2.4" />
823+
<Sleep msec="125"/>
824+
<SetBlackboard output_key="pos" value="22.0;22.0" />
825+
</Sequence>
826+
</Repeat>
827+
</Sequence>
828+
</BehaviorTree>
829+
<BehaviorTree ID="UpdPosPlus">
830+
<Sequence>
831+
<SetBlackboard output_key="pos" value="3.0;5.0" />
832+
<SetBlackboard output_key="pos" value="{new_pos}" />
833+
</Sequence>
834+
</BehaviorTree>
835+
</root>
836+
)";
837+
838+
factory.registerNodeType<UpdatePosition>("UpdatePosition");
839+
factory.registerBehaviorTreeFromText(xml_text);
840+
auto tree = factory.createTree("MainTree");
841+
auto& blackboard = tree.subtrees.front()->blackboard;
842+
843+
// First tick should succeed and update the value within the subtree
844+
ASSERT_NO_THROW(tree.tickExactlyOnce());
845+
846+
const auto entry_ptr = blackboard->getEntry("pos");
847+
ASSERT_EQ(entry_ptr->value.type(), typeid(Point));
848+
849+
const auto x = entry_ptr->value.cast<Point>().x;
850+
const auto y = entry_ptr->value.cast<Point>().y;
851+
ASSERT_EQ(x, 2.2);
852+
ASSERT_EQ(y, 2.4);
853+
854+
// Tick till the end with no crashes
855+
ASSERT_NO_THROW(tree.tickWhileRunning(););
856+
}

0 commit comments

Comments
 (0)