Skip to content

Commit c36bd4d

Browse files
Fix #969: LoopNode accepts std::vector<T> in addition to SharedQueue<T> (#1074)
1 parent 51df1e0 commit c36bd4d

2 files changed

Lines changed: 119 additions & 3 deletions

File tree

‎include/behaviortree_cpp/decorators/loop_node.h‎

Lines changed: 35 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include "behaviortree_cpp/decorator_node.h"
1616

1717
#include <deque>
18+
#include <vector>
1819

1920
namespace BT
2021
{
@@ -75,7 +76,37 @@ class LoopNode : public DecoratorNode
7576
static_queue_ ? AnyPtrLocked() : getLockedPortContent("queue");
7677
if(any_ref)
7778
{
78-
current_queue_ = any_ref.get()->cast<SharedQueue<T>>();
79+
// Try SharedQueue<T> first, then fall back to std::vector<T>.
80+
// This allows upstream nodes that output vector<T> to be used
81+
// directly with LoopNode without manual conversion. Issue #969.
82+
auto queue_result = any_ref.get()->tryCast<SharedQueue<T>>();
83+
if(queue_result)
84+
{
85+
current_queue_ = queue_result.value();
86+
}
87+
else if(!current_queue_)
88+
{
89+
// Only convert on first read; after that, use the
90+
// already-converted current_queue_ (which gets popped).
91+
auto vec_result = any_ref.get()->tryCast<std::vector<T>>();
92+
if(vec_result)
93+
{
94+
// Accept std::vector<T> from upstream nodes. Issue #969.
95+
const auto& vec = vec_result.value();
96+
current_queue_ = std::make_shared<std::deque<T>>(vec.begin(), vec.end());
97+
}
98+
else if(any_ref.get()->isString())
99+
{
100+
// Accept string values (e.g. from subtree remapping). Issue #1065.
101+
auto str = any_ref.get()->cast<std::string>();
102+
current_queue_ = convertFromString<SharedQueue<T>>(str);
103+
}
104+
else
105+
{
106+
throw RuntimeError("LoopNode: port 'queue' must contain either "
107+
"SharedQueue<T>, std::vector<T>, or a string");
108+
}
109+
}
79110
}
80111

81112
if(current_queue_ && !current_queue_->empty())
@@ -114,8 +145,9 @@ class LoopNode : public DecoratorNode
114145

115146
static PortsList providedPorts()
116147
{
117-
// we mark "queue" as BidirectionalPort, because the original element is modified
118-
return { BidirectionalPort<SharedQueue<T>>("queue"),
148+
// Use an untyped BidirectionalPort to accept both SharedQueue<T>
149+
// and std::vector<T> without triggering a port type mismatch. Issue #969.
150+
return { BidirectionalPort("queue"),
119151
InputPort<NodeStatus>("if_empty", NodeStatus::SUCCESS,
120152
"Status to return if queue is empty: "
121153
"SUCCESS, FAILURE, SKIPPED"),

‎tests/gtest_ports.cpp‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -581,3 +581,87 @@ TEST(PortTest, DefaultWronglyOverriden)
581581
// This is correct
582582
ASSERT_NO_THROW(auto tree = factory.createTreeFromText(xml_txt_correct));
583583
}
584+
585+
// Helper class used by Issue #969 test
586+
class CollectDoubleAction : public SyncActionNode
587+
{
588+
public:
589+
CollectDoubleAction(const std::string& name, const NodeConfig& config,
590+
std::vector<double>* collected)
591+
: SyncActionNode(name, config), collected_(collected)
592+
{}
593+
594+
NodeStatus tick() override
595+
{
596+
double val = 0;
597+
if(getInput("value", val))
598+
{
599+
collected_->push_back(val);
600+
return NodeStatus::SUCCESS;
601+
}
602+
return NodeStatus::FAILURE;
603+
}
604+
605+
static PortsList providedPorts()
606+
{
607+
return { BT::InputPort<double>("value") };
608+
}
609+
610+
private:
611+
std::vector<double>* collected_;
612+
};
613+
614+
// Issue #969: LoopNode<T> uses SharedQueue<T> (shared_ptr<deque<T>>) for its queue
615+
// port, but upstream nodes often produce std::vector<T>. This type mismatch causes
616+
// tree creation to fail.
617+
class ProduceVectorDoubleAction : public SyncActionNode
618+
{
619+
public:
620+
ProduceVectorDoubleAction(const std::string& name, const NodeConfig& config)
621+
: SyncActionNode(name, config)
622+
{}
623+
624+
NodeStatus tick() override
625+
{
626+
std::vector<double> vec = { 10.0, 20.0, 30.0 };
627+
setOutput("numbers", vec);
628+
return NodeStatus::SUCCESS;
629+
}
630+
631+
static PortsList providedPorts()
632+
{
633+
return { BT::OutputPort<std::vector<double>>("numbers") };
634+
}
635+
};
636+
637+
TEST(PortTest, LoopNodeAcceptsVector_Issue969)
638+
{
639+
// An upstream node outputs std::vector<double>, and LoopDouble should be
640+
// able to iterate over it without requiring manual conversion to SharedQueue.
641+
std::string xml_txt = R"(
642+
<root BTCPP_format="4">
643+
<BehaviorTree ID="MainTree">
644+
<Sequence>
645+
<ProduceVectorDouble numbers="{nums}" />
646+
<LoopDouble queue="{nums}" value="{val}">
647+
<CollectDouble value="{val}" />
648+
</LoopDouble>
649+
</Sequence>
650+
</BehaviorTree>
651+
</root>
652+
)";
653+
654+
std::vector<double> collected;
655+
656+
BehaviorTreeFactory factory;
657+
factory.registerNodeType<ProduceVectorDoubleAction>("ProduceVectorDouble");
658+
factory.registerNodeType<CollectDoubleAction>("CollectDouble", &collected);
659+
auto tree = factory.createTreeFromText(xml_txt);
660+
auto status = tree.tickWhileRunning();
661+
662+
ASSERT_EQ(status, NodeStatus::SUCCESS);
663+
ASSERT_EQ(collected.size(), 3u);
664+
EXPECT_DOUBLE_EQ(collected[0], 10.0);
665+
EXPECT_DOUBLE_EQ(collected[1], 20.0);
666+
EXPECT_DOUBLE_EQ(collected[2], 30.0);
667+
}

0 commit comments

Comments
 (0)