Port remapping issue

[facontidavide]

From here: ros-navigation/navigation2#4004

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]

Fixed and new tag 3.8.6