BehaviorTree/BehaviorTree.ROS2

Late subscribtions to topics using a dynamic 'topic_name' (blackboardpointer) output 'nullptr' until next message is published.

Closed this issue · 3 comments

When using the RosTopicSubNode object and providing the topic_name port using a blackboard pointer, the subscriber is created on the first tick. When using topics with a transient local setting I would expect the last message on the topic to be provided as the last_msg argument on that same first tick. Instead we keep getting nullptr's.

I've been playing around with multiple settings on either subscription as publisher side, but haven't been able to resolve my problem.
The problem only occurs on the very first tick or ticks until the next message is published on the topics.

Is there a way to fix this?
I heard from my colleagues this behavior also occurs for action/service nodes using blackboard pointers. for action_name and service_name. We haven't been testing publisher nodes, but perhaps the issue is present there as well.

Reproducability
I can reproduce my issue as follows:

  1. Start publishing (transient local) messages
    ros2 topic pub --qos-durability=transient_local --qos-history=keep_last --qos-depth=1 test_topic std_msgs/msg/Bool "{data: False}"
  2. Start behaviortree

Used behaviorTree

<root BTCPP_format="4" main_tree_to_execute="MainTree">
    <BehaviorTree ID="test_tree">
        <GetDigitalSignal topic_name="{test_topic_name}" value="{output}"/>
    </BehaviorTree>

    <BehaviorTree ID="MainTree">
        <Sequence>
	    <Script code="my_topic:='test_topic'"/>
            <Delay delay_msec="10000"> <!-- Added Delay to allow some time connecting to Groot-->
                <KeepRunningUntilFailure>
                    <ForceSuccess>
                        <Delay delay_msec="3000"> <!-- Delay added to see in Groot what happens easier-->
                            <ParallelAll>
                                <GetDigitalSignal topic_name="test_topic" value="{my_output_static}"/>
                                <GetDigitalSignal topic_name="{my_topic}" value="{my_output_scripted}"/>
                                <SubTree ID="test_tree" test_topic_name="{my_topic}" output="{my_ouput_subtree}"/>
                            </ParallelAll>
                        </Delay>
                    </ForceSuccess>
                </KeepRunningUntilFailure>
            </Delay>
        </Sequence>
    </BehaviorTree>
</root>

GetDigitalSignal Implementation

static BT::PortsList providedPorts()
{
    return providedBasicPorts({
        BT::OutputPort<std_msgs::msg::Bool::_data_type>("value", "Current value of the digital signal"),
    });
}

BT::NodeStatus onTick(const std_msgs::msg::Bool::SharedPtr &msg)
{
    if (msg != nullptr)
    {
        setOutput("value", msg->data);
        return BT::NodeStatus::SUCCESS;
    }

    return BT::NodeStatus::FAILURE;
}

Groot Outcome
image

I think this is due to the subscriber itself being implemented as a singleton, and then when the connection is made with the bt action, the already received message is not pushed onto the queue of the new action

Hi @facontidavide,
Did you by any chance found some time already to look into / reproduce this issue?
Thanks in advance!

a PR was merged to fix this. closing