Best practice to use BehaviorTree.CPP along with ROS

[maker-ATOM]

My ultimate goal is to use BehaviorTree in conjugation with ROS specfically move_base to control a robot move from one point to other.

What I am doing (For testing purpose):
To be more specific I have a StatefulActionNode (work) which get information about current status of goal be it RUNNING, ABORTED or SUCCESS. For time being the BehaviorTree has only a single StatefulActionNode. Functions for condition node are defined inside the
work class.

ROS methods primarily subscription callback are also defined within the work class.

So basically all methods be it related to BehaviorTree or ROS all are defined under single class work which is inherited from StatefulActionNode, the reason to do this is because in case of multiple subscriptions, methods of the same class can access data elements directly.

This is the Tree I am using,

<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
  <BehaviorTree ID="bt">
    <Sequence>
      <Switch/>
      <Work/>
    </Sequence>
  </BehaviorTree>

  <!-- Description of Node Models (used by Groot) -->
  <TreeNodesModel>
    <Condition ID="Switch"
               editable="true"/>
    <Action ID="Work"
            editable="true"/>
  </TreeNodesModel>

</root>

The XML version of the tree, 👆

#include <iostream>
#include <chrono>

#include <ros/ros.h>
#include <std_msgs/Int8.h>

#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/loggers/groot2_publisher.h"

using namespace std;

class Work : public BT::StatefulActionNode
{
public:
    Work(const std::string &name, const BT::NodeConfig &config)
        : StatefulActionNode(name, config), work_done(0)
    {
    }

    void init_ros(ros::NodeHandle &nh)
    {
        nh_ = nh;
        _state_sub = nh_.subscribe("state", 10, &Work::stateCallback, this);
    }

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<int>("none")};
    }

    BT::NodeStatus onStart() override
    {
        cout << "[Work: ACTIVE]" << endl;
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override
    {
        work_done++;

        cout << "[BT] work_done: " << work_done << endl;
        cout << "[BT] state: " << state << endl;

        if (work_done > 2)
        {
            work_done = 0;
            cout << "[Work: SUCCESS]" << endl;
            return BT::NodeStatus::SUCCESS;
        }
        else
        {
            cout << "[Work: RUNNING]" << endl;
            return BT::NodeStatus::RUNNING;
        }
    }

    void onHalted() override
    {
        cout << "[Work: ABORTED]" << endl;
    }

    BT::NodeStatus Switch()
    {
        std::cout << "[Switch: SUCCESS]" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }

    void stateCallback(const std_msgs::Int8::ConstPtr &State)
    {
        state = State->data;
        std::cout << "[ROS] work_done: " << work_done << std::endl;
        std::cout << "[ROS] state: " << state << std::endl;
    }

private:
    int work_done = 0;
    int state = 0;

    ros::NodeHandle nh_;
    ros::Subscriber _state_sub;
};

int main(int argc, char **argv)
{

    ros::init(argc, argv, "robot_behavior");
    ros::NodeHandle nh;

    string path = "/home/aditya/spark_ws/src/ros_vs_bt/config/ros_vs_bt.xml";

    BT::BehaviorTreeFactory factory;

    Work work("work", {});
    work.init_ros(nh);

    factory.registerNodeType<Work>("Work");
    factory.registerSimpleAction(
        "Switch",
        bind(&Work::Switch, &work));

    auto tree = factory.createTreeFromFile(path);

    BT::Groot2Publisher publisher(tree);

    std::chrono::milliseconds sleep_time = std::chrono::milliseconds(1000);

    BT::NodeStatus status = BT::NodeStatus::IDLE;

    while (true)
    {
        std::cout << "--- ticking\n";
        status = tree.tickOnce();
        ros::spinOnce();
        std::cout << "--- status: " << toStr(status) << "\n\n";
        tree.sleep(sleep_time);
    }
    return 0;
}

And the code that I am using currently,

OUPUT:

--- ticking
[Switch: SUCCESS]
[Work: ACTIVE]
--- status: RUNNING

--- ticking
[BT] work_done: 1
[BT] state: 0
[Work: RUNNING]
[ROS] work_done: 0
[ROS] state: 89
--- status: RUNNING

--- ticking
[BT] work_done: 2
[BT] state: 0
[Work: RUNNING]
[ROS] work_done: 0
[ROS] state: 90
--- status: RUNNING

--- ticking
[BT] work_done: 3
[BT] state: 0
[Work: SUCCESS]
[ROS] work_done: 0
[ROS] state: 91
--- status: SUCCESS

--- ticking
[Switch: SUCCESS]
[Work: ACTIVE]
[ROS] work_done: 0
[ROS] state: 92
--- status: RUNNING

--- ticking
[BT] work_done: 1
[BT] state: 0
[Work: RUNNING]
[ROS] work_done: 0
[ROS] state: 93
--- status: RUNNING

--- ticking
[BT] work_done: 2
[BT] state: 0
[Work: RUNNING]
[ROS] work_done: 0
[ROS] state: 94
--- status: RUNNING

The while loop ticks the behavior tree and ros once every iteration, the node subscribes to topic state and stores the value published on the topic within the class attribute. Moreover work node also does something similar increments another class attribute every time it is ticked.

Since both functions are methods of the same class the attributes should accessible which is not the case. BehaviorTree node cannot get the latest value updated by the ROS subscription callback and ROS subscription callback cannot get the latest value updated by the Behavior Tree node.

Meaning in the last block of output behavior tree say value of state variable is 0 ([BT] state: 0) which actually is 94 and subscription callback says value of work_done incremented by BehaviorTree is 0 ([ROS] work_done: 0) which actually is 2.

So what is the best practice to have BehaviorTree Nodes and ROS methods defined in way so that data exchange between them happens?