can ros subscriber callback work under behavior tree?

[YuLin1226]

trafficstars

Hi, I'm new in behavior tree. I write a ROS1 cpp code to navigation robot to follow forward path and then backward path. Navigation part can bo done with navigation stack, so what I need to do is just pub path goal. I use behavior tree to form a task flow listed below:

1. Pub forward path.
2. Listen to topic to check if robot reach goal.
3. Pub backward path.
4. Listen to topic to check if robot reach goal.

So this is a simple behavior tree with a ros publisher and subscriber. My problem is that under the behavior tree, my subscriber callback does change the value of navSucceed_ in my code, but the while loop won't be stopped so that the whole process cannot move to step3 (stuck in step2).

Here are my cpp codes: lib.cpp

#include "../../include/nav_bt_task/setup/nav_task_1.h"
#include "nav_msgs/Path.h"
#include "tf2/LinearMath/Quaternion.h"

namespace NavTask
{
    // Action Declaration.
    NodeStatus TaskOne::actionNavForwardPath()
    {
        ROS_INFO("Forward path navigation.");
        pubForwardPath();
        return NodeStatus::SUCCESS;
    }

    NodeStatus TaskOne::actionNavBackwardPath()
    {
        ROS_INFO("Backward path navigation.");
        pubBackwardPath();
        return NodeStatus::SUCCESS;
    }

    NodeStatus TaskOne::actionPrintReachGoal()
    {
        std::cout << navSucceed_ << std::endl;
        ros::spin();
        while(!navSucceed_)
        {
            ros::spin();
        }
        ROS_INFO("Goal reached.");
        return NodeStatus::SUCCESS;
    }

    void TaskOne::RegisterNodes(BehaviorTreeFactory& factory, NavTask::TaskOne* TaskOne)
    {
        factory.registerSimpleAction("Action_NavForwardPath", std::bind(&TaskOne::actionNavForwardPath, *TaskOne));
        factory.registerSimpleAction("Action_NavBackwardPath", std::bind(&TaskOne::actionNavBackwardPath, *TaskOne));
        factory.registerSimpleAction("Action_GoalReached", std::bind(&TaskOne::actionPrintReachGoal, *TaskOne));
    }
    // 
    TaskOne::TaskOne():
    initialized_(false),
    private_nh_("~"),
    // callbackQueue_(true),
    // asyncSpinner_(NUM_THREADS, &callbackQueue_),
    navSucceed_(false)
    {
        initialize();
    }

    TaskOne::~TaskOne()
    {
    }

    void TaskOne::initialize()
    {
        if(!initialized_)
        {
            // do initialization
            // private_nh_.setCallbackQueue(&callbackQueue_);
            // asyncSpinner_.start();
            waypoints_pub_ = private_nh_.advertise<nav_msgs::Path>("/global_way_points_array", 1);
            nav_goal_result_sub_ = private_nh_.subscribe("/waypoints_received", 10, &TaskOne::subCallback, this);
            ROS_INFO("Initialize Nav Task 1 successfully.");
            initialized_ = true;
        }
        else
        {
            ROS_WARN("Nav Task 1 has been initialized.");
        }
    }

    void TaskOne::pubForwardPath()
    {
        navSucceed_ = false;
        waypoints_.poses.clear();
        addWaypointToVector(1.0, 1.0, 0.0);
        addWaypointToVector(2.0, 2.0, 0.0);
        // Refresh vector header.
        waypoints_.header = waypoints_.poses.back().header;
        // pub
        waypoints_pub_.publish(waypoints_);
    }

    void TaskOne::pubBackwardPath()
    {
        navSucceed_ = false;
        waypoints_.poses.clear();
        addWaypointToVector(1.0, 1.0, 0.0);
        addWaypointToVector(0.0, 0.0, 0.0);
        // Refresh vector header.
        waypoints_.header = waypoints_.poses.back().header;
        // pub
        waypoints_pub_.publish(waypoints_);
    }

    void TaskOne::addWaypointToVector(double pointX, double pointY, double pointYAW)
    {
        ROS_INFO("Add waypoint to vector (x: %f | y: %f | yaw: %f).", pointX, pointY, pointYAW);
        tf2::Quaternion q;
        // Create this quaternion from roll/pitch/yaw (in radians)
        q.setRPY(0, 0, pointYAW);  
        q.normalize();
        
        geometry_msgs::PoseStamped waypoint;
        waypoints_.poses.push_back(waypoint);
        if(waypoints_.poses.size() > 1)
            waypoints_.poses.back().header.seq = waypoints_.poses.end()[-2].header.seq + 1;
        else
            waypoints_.poses.back().header.seq = 0;
        waypoints_.poses.back().header.stamp = ros::Time::now();
        waypoints_.poses.back().header.frame_id = "map";

        waypoints_.poses.back().pose.position.x = pointX;
        waypoints_.poses.back().pose.position.y = pointY;
        waypoints_.poses.back().pose.position.z = 0.0;
        waypoints_.poses.back().pose.orientation.x = q.getX();
        waypoints_.poses.back().pose.orientation.y = q.getY();
        waypoints_.poses.back().pose.orientation.z = q.getZ();
        waypoints_.poses.back().pose.orientation.w = q.getW();
    }

    void TaskOne::subCallback(const move_base_msgs::MoveBaseActionResult::ConstPtr& result)
    {
        ROS_INFO("Get result: %i", result->status.status);
        if(result->status.status == 3)
            navSucceed_ = true;
        else
            navSucceed_ = false;
    }

}// namespace

main.cpp

#include "../include/nav_bt_task/setup/nav_task_1.h"

#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
#include "behaviortree_cpp_v3/loggers/bt_minitrace_logger.h"
#include "behaviortree_cpp_v3/loggers/bt_file_logger.h"
#include "behaviortree_cpp_v3/bt_factory.h"

#ifdef ZMQ_FOUND
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
#endif

static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree">
    <BehaviorTree ID="MainTree">
        <Sequence>
            <Action_NavForwardPath/>
            <Action_GoalReached/>
            <Action_NavBackwardPath/>
        </Sequence>
    </BehaviorTree>
</root>
 )";

using namespace BT;

int main(int argc, char** argv)
{
    ros::init(argc,argv,"my_node_name");
    BT::BehaviorTreeFactory factory;
    NavTask::TaskOne task;
    NavTask::TaskOne::RegisterNodes(factory, &task);
    auto tree = factory.createTreeFromText(xml_text);
    StdCoutLogger logger_cout(tree);
    FileLogger logger_file(tree, "nav_task_1_trace.fbl");
    MinitraceLogger logger_minitrace(tree, "nav_task_1_trace.json");

#ifdef ZMQ_FOUND
    PublisherZMQ publisher_zmq(tree);
#endif

    printTreeRecursively(tree.rootNode());
    const bool LOOP = ( argc == 2 && strcmp( argv[1], "loop") == 0);
    do
    {
        NodeStatus status = NodeStatus::RUNNING;
        while(status == NodeStatus::RUNNING)
        {
            // ros::spinOnce();
            status = tree.tickRoot();
            NavTask::SleepMS(1);   // optional sleep to avoid "busy loops"
        }
        NavTask::SleepMS(1000);
    }
    while(LOOP);

    return 0;
}

Any suggestion? Thanks.

[YuLin1226]

BTW, another problem: can callback queue and asyncSpinner work with behavior tree? If needed, I can post this problem in another open issue. Thanks.

[SteveMacenski]

Checkout ROS 2' Nav2 stack for examples of calling actions / subscribers / callbacks within BT nodes.

@facontidavide I think you can close this one by example

[facontidavide]

Please, see here: https://github.com/BehaviorTree/BehaviorTree.ROS2/