BT "node" port remapping does not work with subtrees

[AndyZe]

Bug report

Required Info:

• Operating System:
  Ubuntu 22
• ROS2 Version:
  Humble
• Version or commit hash:
  • nav2-behavior-tree is version 1.1.12-1jammy.20231117.204507
• DDS implementation:
  • Default

Steps to reproduce issue

This should be considered low priority. It's easy to work around by simply not using BT subtrees.

I was trying to use a subtree which included a DriveOnHeading nav node, like so:

<?xml version='1.0' encoding='UTF-8'?>
<root BTCPP_format="4">

<BehaviorTree ID="MySubTree">
    <Sequence>
        <DriveOnHeading dist_to_travel="3" speed="0.05" time_allowance="60"/>
    </Sequence>
</BehaviorTree>
</root>

The parent tree defines the node port and should remap it for the subtree like so:

<BehaviorTree ID="MyParentTree">
    <Sequence>
        <Initialize node="{node}"/>
        ...
        <SubTree ID="MySubTree" node="{node}"/>
        ...

However there is still this error at runtime:

[my_executable-5] terminate called after throwing an instance of 'BT::RuntimeError'
[my_executable-5]   what():  Blackboard::get() error. Missing key [node]

It works fine if I simply don't use the subtree but rather combine everything in one large BT.

Suspect it's related to this line:

https://github.com/ros-planning/navigation2/blob/113564965f54009686d521902ff3fcc9d101c5b5/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp#L52

[SteveMacenski]

This is fixed in Humble in source https://github.com/ros-planning/navigation2/blame/humble/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp#L188-L193, it just hasn't been released into binary format yet. Can you confirm source builds of Nav2 on Humble resolves your issue?

[AndyZe]

Sorry to say, humble branch seems to have the same issue. I just built 3ed4c2d from Nov 6th (the latest)

[SteveMacenski]

@facontidavide, @AndyZe is saying that the soln in #3640 didn't work to share the subtree blackboard members (which seems odd to me). Anything that seems clear to you to be the cause? Does the blackboard_stack contain all of the subtree blackboards? If so, I can't see how this would be a problem still.

[jncfa]

@facontidavide, @AndyZe is saying that the soln in #3640 didn't work to share the subtree blackboard members (which seems odd to me). Anything that seems clear to you to be the cause? Does the blackboard_stack contain all of the subtree blackboards? If so, I can't see how this would be a problem still.

Not sure if this is correct, but it looks like BtActionNode is fetching the key from blackboard in the BtActionNode constructor, when the tree is being created in createTreeFromFile(). However, you are only traversing the blackboard stack and setting the blackboard values manually after the tree is created

[SteveMacenski]

Ah, that is a good catch, but we do give createTreeFromFile the blackboard to use, so that should be used. I think we've done everything we can do on an external basis to provide it, it requires adjustments to BT.CPP. It does look to me like that blackboard is given to the subtrees though https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/xml_parsing.cpp#L475 -- but overrided if this variable isn't set: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/xml_parsing.cpp#L683. So perhaps this would work actually if you set that shared blackboard variable to true?

We unfortunately do require that those are done in the constructor.

[jncfa]

Ah, that is a good catch, but we do give createTreeFromFile the blackboard to use, so that should be used. I think we've done everything we can do on an external basis to provide it, it requires adjustments to BT.CPP. It does look to me like that blackboard is given to the subtrees though https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/xml_parsing.cpp#L475 -- but overrided if this variable isn't set: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/xml_parsing.cpp#L683. So perhaps this would work actually if you set that shared blackboard variable to true?

We unfortunately do require that those are done in the constructor.

You should be able to "fix" it by either enabling __shared_blackboard or __autoremap.

If I recall correctly from when I looked at this problem before, the tldr is that a key is only available on the blackboard of a subtree if:

• Some node in the tree is reading it for an input port (in the XML);
• You have a subtree in your tree that has a node that is reading it for an input port (in the XML);
• Or you have auto-remapping/shared blackboard shenanigans enabled.

From the source code, you can see that a Blackboard::get() will fetch the key from the current blackboard storage first, and if it is not found, it will look in the parent blackboard only if autoremapping is enabled:
https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/include/behaviortree_cpp_v3/blackboard.h#L51-L68

A key is written to the blackboard via the createEntry()/createEntryImpl() method: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/blackboard.cpp#L68-L116

You can observe that if you call createEntryImpl() for a key that is remapped from a parent tree (or autoremapped), it will recursively call createEntryImpl until you reach the root tree's blackboard.

On the XML parsing side, you have that blackboard->createEntry() is only called if a key is declared in node's manifest: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/v3.8/src/xml_parsing.cpp#L557-L601

EDIT: Also, I believe this only applies to v3.8.5, I'm not 100% sure but I believe it works out of the box on v4.4

[facontidavide]

@SteveMacenski knows what I think about sharing the node using the blackboard in that way 😉
That is an anti-pattern in the way blackboard is used and it is not supported.

The way a node should be shared is documented in Tutorial 8 and demonstrated in BehaviorTree.ROS2.

Said that, I am happy to help 😄

• _shared_blackboard is supposed to work in version 3.8 at the expense of breaking the private scoping of the subtree.
• The fix mentioned by Steve should work.

If any of these two statements are not true, I will be happy to investigate, but I would greatly appreciate if @AndyZe can share a unit test of "hello world" example to reproduce the issue.

As you can see here, createEntryImpl is called when using Blackboard::set, that is what Nav2 does, I believe.

[jncfa]

@SteveMacenski knows what I think about sharing the node using the blackboard in that way 😉 That is an anti-pattern in the way blackboard is used and it is not supported.

The way a node should be shared is documented in Tutorial 8 and demonstrated in BehaviorTree.ROS2.

Said that, I am happy to help 😄

* `_shared_blackboard` is supposed to work in version 3.8 at the expense of breaking the private scoping of the subtree.

* The fix mentioned by Steve should work.

If any of these two statements are not true, I will be happy to investigate, but I would greatly appreciate if @AndyZe can share a unit test of "hello world" example to reproduce the issue.

As you can see here, createEntryImpl is called when using Blackboard::set, that is what Nav2 does, I believe.

Are you referring to this fix?

tree_ = bt_->createTreeFromFile(filename);
for(auto& blackboard: tree.blackboard_stack)
{
  blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);  
  blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);  
  blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);   
}

So, from my understanding, createTreeFromFile will instantiate the tree and all BT nodes declared on the XML.

Please correct me if I'm wrong @facontidavide, but to me the problem seems to be on the constructor of the offending node, not when it ticks, so when you traverse the blackboard stack to ensure all blackboards have the required entries it is already too late.
You would need a solution that ensures the entries exist when the node is being constructed.

If you use either __shared_blackboard or __autoremap, this will work:

• Enabling _shared_blackboard passes the parent blackboard to its child subtree when constructing it, so you will have the entries there when the node tries to fetch from the blackboard;
• Enabling _autoremap allows entries to be fetched from the parent if not found on the current blackboard, which would also prevent this error from happening.

The underlying problem however seems to be that just manually remapping a key to a subtree does not guarantee it is written to the subtree's blackboard storage unless there is at least one node in the subtree that is using it. This was the conclusion I got to when I looked at this problem, but please correct me if I misunderstood something here

I understand that Nav2 is not using the library as you intended, hence why these issues keep popping up. I'm just trying to help here 😄

[facontidavide]

So, from my understanding, createTreeFromFile will instantiate the tree and all BT nodes declared on the XML.

correct.

so when you traverse the blackboard stack to ensure all blackboards have the required entries it is already too late.

😢 you are right, I stand corrected.

You would need a solution that ensures the entries exist when the node is being constructed.

The solution is to change the code to follow the pattern in Tutorial 8

The underlying problem however seems to be that just manually remapping a key to a subtree does not guarantee it is written to the subtree's blackboard storage unless there is at least one node in the subtree that is using it.

Let me update this unit test. Since that is not my intent, once confirmed, I will correct it, also in branch 3.8 👍

[jncfa]

So, from my understanding, createTreeFromFile will instantiate the tree and all BT nodes declared on the XML.

correct.

so when you traverse the blackboard stack to ensure all blackboards have the required entries it is already too late.

😢 you are right, I stand corrected.

You would need a solution that ensures the entries exist when the node is being constructed.

The solution is to change the code to follow the pattern in Tutorial 8

The underlying problem however seems to be that just manually remapping a key to a subtree does not guarantee it is written to the subtree's blackboard storage unless there is at least one node in the subtree that is using it.

Let me update this unit test. Since that is not my intent, once confirmed, I will correct it, also in branch 3.8 👍

In case you still need it (patch for v3.8 branch of BehaviorTree.CPP nav2_subtree_patch.zip):

TEST(SubTree, SubtreePlusNav2Fails)
{
  static const char* xml_text = R"(
<root main_tree_to_execute="Tree1">

<BehaviorTree ID="Tree1">
  <Sequence>
    <SubTreePlus ID="Tree2" ros_node="{ros_node}"/>
  </Sequence>
</BehaviorTree>

<BehaviorTree ID="Tree2">
    <SubTreePlus ID="Tree3" ros_node="{ros_node}"/>
</BehaviorTree>

<BehaviorTree ID="Tree3">
    <SubTreePlus ID="Talker" ros_node="{ros_node}"/>
</BehaviorTree>

<BehaviorTree ID="Talker">
  <Sequence>
    <NaughtyNav2Node/>
  </Sequence>
</BehaviorTree>

</root>)";

  BehaviorTreeFactory factory;
  factory.registerNodeType<NaughtyNav2Node>("NaughtyNav2Node");

  auto blackboard = BT::Blackboard::create();
  blackboard->set<std::string>("ros_node", "nav2_shouldnt_do_this");

  EXPECT_THROW(factory.createTreeFromText(xml_text, blackboard), RuntimeError);
}

TEST(SubTree, SubtreePlusNav2Works)
{
  static const char* xml_text = R"(
<root main_tree_to_execute="Tree1">

<BehaviorTree ID="Tree1">
  <Sequence>
    <SubTreePlus ID="Tree2" ros_node="{ros_node}"/>
  </Sequence>
</BehaviorTree>

<BehaviorTree ID="Tree2">
    <SubTreePlus ID="Tree3" ros_node="{ros_node}"/>
</BehaviorTree>

<BehaviorTree ID="Tree3">
    <SubTreePlus ID="Talker" ros_node="{ros_node}"/>
</BehaviorTree>

<BehaviorTree ID="Talker">
  <Sequence>
    <SaySomething message="{ros_node}" />
    <NaughtyNav2Node/>
  </Sequence>
</BehaviorTree>

</root>)";

  BehaviorTreeFactory factory;
  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
  factory.registerNodeType<NaughtyNav2Node>("NaughtyNav2Node");

  auto blackboard = BT::Blackboard::create();
  blackboard->set<std::string>("ros_node", "nav2_shouldnt_do_this");

  Tree tree = factory.createTreeFromText(xml_text, blackboard);

  auto ret = tree.tickRoot();
  ASSERT_EQ(ret, NodeStatus::SUCCESS);
}

TEST(SubTree, SubtreeNav2Fails)
{
  static const char* xml_text = R"(
<root main_tree_to_execute="Tree1">

<BehaviorTree ID="Tree1">
  <Sequence>
    <SubTree ID="Tree2" ros_node="ros_node"/>
  </Sequence>
</BehaviorTree>

<BehaviorTree ID="Tree2">
    <SubTree ID="Tree3" ros_node="ros_node"/>
</BehaviorTree>

<BehaviorTree ID="Tree3">
    <SubTree ID="Talker" ros_node="ros_node"/>
</BehaviorTree>

<BehaviorTree ID="Talker">
  <Sequence>
    <NaughtyNav2Node/>
  </Sequence>
</BehaviorTree>

</root>)";

  BehaviorTreeFactory factory;
  factory.registerNodeType<NaughtyNav2Node>("NaughtyNav2Node");

  auto blackboard = BT::Blackboard::create();
  blackboard->set<std::string>("ros_node", "nav2_shouldnt_do_this");

  EXPECT_THROW(factory.createTreeFromText(xml_text, blackboard), RuntimeError);
}

TEST(SubTree, SubtreeNav2Works)
{
  static const char* xml_text = R"(
<root main_tree_to_execute="Tree1">

<BehaviorTree ID="Tree1">
  <Sequence>
    <SubTree ID="Tree2" ros_node="ros_node"/>
  </Sequence>
</BehaviorTree>

<BehaviorTree ID="Tree2">
    <SubTree ID="Tree3" ros_node="ros_node"/>
</BehaviorTree>

<BehaviorTree ID="Tree3">
    <SubTree ID="Talker" ros_node="ros_node"/>
</BehaviorTree>

<BehaviorTree ID="Talker">
  <Sequence>
    <SaySomething message="{ros_node}" />
    <NaughtyNav2Node/>
  </Sequence>
</BehaviorTree>

</root>)";

  BehaviorTreeFactory factory;
  factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
  factory.registerNodeType<NaughtyNav2Node>("NaughtyNav2Node");

  auto blackboard = BT::Blackboard::create();
  blackboard->set<std::string>("ros_node", "nav2_shouldnt_do_this");

  Tree tree = factory.createTreeFromText(xml_text, blackboard);

  auto ret = tree.tickRoot();
  ASSERT_EQ(ret, NodeStatus::SUCCESS);
}

[facontidavide]

moved the issue here: BehaviorTree/BehaviorTree.CPP#724

[SteveMacenski]

OK - closing here then since covered in BT.CPP. Thanks for the good discussion!

[facontidavide]

FYI, the solution was pushed and can be found in version 3.8.6

[facontidavide]

Note for @AndyZe : you MUST use SubTreePlus, otherwise the remapping syntax in your example won't work

[AndyZe]

Thank you!