Bug fix/set blackboard

[devis12]

SetBlackboard action bug fix

Previous SetBlackboard action node did not update the sequence id, the timestamp when called on a pre-existing entry.
Moreover, it allowed to bypass type checking.

This occurred only when the input was a blackboard pointer.

By using the BT::Blackboard::set method I believe also is thread safe which before was not, but I provided test that were failing previously and now are passing exclusively for the two cases above

[devis12]

Just fyi...

This bug made me notice a couple of stuff that are not handled correctly in the current resolution of the remapped values imho...

Take this tree as a very silly example

<?xml version="1.0"?>
<root BTCPP_format="4" main_tree_to_execute="BehaviorTree">
    <!-- ////////// -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <SetBlackboard output_key="pos" value="0.0;0.0" />
            <KeepRunningUntilFailure>
                <ForceSuccess>
                    <Sequence>
                        <UpdatePosition pos_in="{pos}" x="0.1" y="0.2" pos_out="{pos}"/>
                        <SetBlackboard output_key="pos" value="22.0;22.0" />
                        <Sleep msec="1000"/>
                    </Sequence>
                </ForceSuccess>
            </KeepRunningUntilFailure>
        </Sequence>
    </BehaviorTree>
    <BehaviorTree ID="A">
        <Sequence>
            <SetBlackboard output_key="pos" value="3.0;5.0" />
            <SetBlackboard output_key="pos" value="{new_pos}" />
        </Sequence>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <SubTree ID="BehaviorTree" description=""/>
        <SubTree ID="A" description="" />
    </TreeNodesModel>
    <!-- ////////// -->
</root>

It's a no sense tree, you can quickly mockup UpdatePosition with the new features available in the library or just use this silly implementation:

// Simple Action that updates an instance of Position2D 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<Position2D>("pos_in");
    if(!in_pos.has_value())
      return BT::NodeStatus::FAILURE;
    Position2D _pos = in_pos.value();
    _pos.x += getInput<double>("x").value_or(0.0);
    _pos.y += getInput<double>("y").value_or(0.0);
    std::cout << "Updating pos to " << std::to_string(_pos.x) << ", " << std::to_string(_pos.y) << std::endl;
    setOutput("pos_out", _pos);
    return BT::NodeStatus::SUCCESS;
  }

  static BT::PortsList providedPorts()
  {
    return { 
        BT::InputPort<Position2D>("pos_in", {0.0, 0.0}, "Initial position"),
        BT::InputPort<double>("x"),
        BT::InputPort<double>("y"), 
        BT::OutputPort<Position2D>("pos_out") 
    };
  }

private:
};

This tree will iterate undefinitely and, while the first time the loop artificially created with the KeepRunningUntilFailure and ForceSuccess combo runs, the second the tree will crash. Why?

• After instantiation of the tree, the entry pos in the root blackboard has type Position2D type
• With the first SetBlackboard in the SubTree "A" we would need to resolve the remapping given by the _autoremap=true through createEntryImpl method called without a strong type essentially, a string any is force there....
• the next time we reach the UpdatePosition node in the second iteration of this infinite loop, we will in a crash in the copyInto that is performed in the blackboard::set

Note that you can reproduce this bug in the original version without my modified line. Will build the test soon and try to think about a solution without breaking too much the current logic...