ros action and condition nodes not running when behavior tree is running fine

[Evintjh]

Hi, my behavior tree is running fine but my ros nodes are not. I have verified with action_client and condition_client that they are working. @miccol Would you know the cause of that? These are my code below:

behavior tree:

#include <behavior_tree.h>
#include <actions/action_test_node.h>
#include <conditions/condition_test_node.h>
#include <iostream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "BehaviorTree");
    try
    {
        int TickPeriod_milliseconds = 1000;


        // BT::ROSAction* action = new BT::ROSAction("action");
        // BT::NegationNode* decorator = new BT::NegationNode("decorator");
        // BT::ROSCondition* condition = new BT::ROSCondition("condition");


        // BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1");

        // sequence1->AddChild(decorator);
        // decorator->AddChild(condition);
        // sequence1->AddChild(action);
        BT::ActionTestNode* action = new BT::ActionTestNode("action_ros");
        // BT::NegationNode* decorator = new BT::NegationNode("decorator");
        BT::ConditionTestNode* condition = new BT::ConditionTestNode("condition_ros");


        BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1");
        std::cout << "sequence with no memory" << std::endl;

        condition->set_boolean_value(true);
        sequence1->AddChild(condition);
        // sequence1->AddChild(decorator);
        // decorator->AddChild(condition);
        sequence1->AddChild(action);

        Execute(sequence1, TickPeriod_milliseconds);  // from BehaviorTree.cpp
    }
    catch (BT::BehaviorTreeException& Exception)
    {
        std::cout << Exception.what() << std::endl;
    }

    return 0;
}

behavior tree action node:

#include <actions/action_test_node.h>
#include <string>
#include <iostream>


BT::ActionTestNode::ActionTestNode(std::string name) : ActionNode::ActionNode(name)
{
    type_ = BT::ACTION_NODE;
    boolean_value_ = true;
    time_ = 3;
    thread_ = std::thread(&ActionTestNode::WaitForTick, this);
}

BT::ActionTestNode::~ActionTestNode() {}

void BT::ActionTestNode::WaitForTick()
{
    while (true)
    {
        // Waiting for the first tick to come
        DEBUG_STDOUT(get_name() << " WAIT FOR TICK");

        tick_engine.Wait();
        DEBUG_STDOUT(get_name() << " TICK RECEIVED");

        // Running state
        set_status(BT::RUNNING);
        // Perform action...
        int i = 0;
        int j = 0;
        while (get_status() != BT::HALTED && i++ < time_)
        {   
            // where you place your algorithm
            for (j; j<5; j++){
                std::cout<< "j is: "<< j << std::endl;
            }
            DEBUG_STDOUT(" Action " << get_name() << "running! Thread id:" << std::this_thread::get_id());
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
        if (get_status() != BT::HALTED)
        {
            if (boolean_value_)
            {
                set_status(BT::SUCCESS);
                DEBUG_STDOUT(" Action " << get_name() << " Done!");
                
            }
            else
            {
                set_status(BT::FAILURE);
                DEBUG_STDOUT(" Action " << get_name() << " FAILED!");
            }
        }
    }
}

void BT::ActionTestNode::Halt()
{
    set_status(BT::HALTED);
    DEBUG_STDOUT("HALTED state set!");
}


void BT::ActionTestNode::set_time(int time)
{
    time_ = time;
}



void BT::ActionTestNode::set_boolean_value(bool boolean_value)
{
    boolean_value_ = boolean_value;
}

behavior tree condition node:

#include <conditions/condition_test_node.h>
#include <string>

BT::ConditionTestNode::ConditionTestNode(std::string name) : ConditionNode::ConditionNode(name)
{
    type_ = BT::CONDITION_NODE;
    boolean_value_ = true;
}

BT::ConditionTestNode::~ConditionTestNode() {}

BT::ReturnStatus BT::ConditionTestNode::Tick()
{
        if (get_status() == BT::EXIT)
        {
            // The behavior tree is going to be destroied
            return BT::EXIT;
        }

        // Condition checking and state update

        if (boolean_value_)
        {
            set_status(BT::SUCCESS);
            std::cout << get_name() << " returning Success" << BT::SUCCESS << "!" << std::endl;
            return BT::SUCCESS;
        }
        else
        {
            set_status(BT::FAILURE);
            std::cout << get_name() << " returning Failure" << BT::FAILURE << "!" << std::endl;
            return BT::FAILURE;
        }
}

ros_action node:

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <behavior_tree_core/BTAction.h>
#include <iostream>
#include <string>


enum Status {RUNNING, SUCCESS, FAILURE};  // BT return status


class BTAction
{
protected:
    ros::NodeHandle nh_;
    // NodeHandle instance must be created before this line. Otherwise strange error may occur.
    actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
    std::string action_name_;
    // create messages that are used to published feedback/result
    behavior_tree_core::BTFeedback feedback_;  // action feedback (SUCCESS, FAILURE)
    behavior_tree_core::BTResult result_;  // action feedback  (same as feedback for us)


public:
    explicit BTAction(std::string name) :
        as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
        action_name_(name)
    {
        // Starts the action server
        as_.start();
    }

    ~BTAction(void)
    {}

    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
    {
        // publish info to the console for the user
        ROS_INFO("Starting Action");

        // start executing the action
        int i = 0;
        while (i < 5)
        {
            // check that preempt has not been requested by the client
            if (as_.isPreemptRequested())
            {
                ROS_INFO("Action Halted");
                std::cout<< "ros i is: "<< i << std::endl;
                // set the action state to preempted
                as_.setPreempted();
                break;
            }
            ROS_INFO("Executing Action");

            ros::Duration(0.5).sleep();  // waiting for 0.5 seconds
            i++;
        }

        if (i == 5)
        {
            set_status(SUCCESS);
        }
    }


    // returns the status to the client (Behavior Tree)
    void set_status(int status)
    {
        // Set The feedback and result of BT.action
        feedback_.status = status;
        result_.status = feedback_.status;
        // publish the feedback
        as_.publishFeedback(feedback_);
        // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
        as_.setSucceeded(result_);

        switch (status)  // Print for convenience
        {
        case SUCCESS:
            ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );
            break;
        case FAILURE:
            ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );
            break;
        default:
            break;
        }
    }
};

ros condition node:

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <behavior_tree_core/BTAction.h>

#include <string>

enum Status {RUNNING, SUCCESS, FAILURE};


class BTAction
{
protected:
    ros::NodeHandle nh_;
    // NodeHandle instance must be created before this line. Otherwise strange errors may occur.
    actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
    std::string action_name_;
    // create messages that are used to published feedback/result
    behavior_tree_core::BTFeedback feedback_;
    behavior_tree_core::BTResult result_;

public:
    explicit BTAction(std::string name) :
        as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
        action_name_(name)
    {
        // start the action server (action in sense of Actionlib not BT action)
        as_.start();
        ROS_INFO("Condition Server Started");
    }

    ~BTAction(void)
    { }
    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
    {
        if (true)
        {
            std::cout<< " ros condition fulfilled"<< std::endl;
            set_status(SUCCESS);
        }
        else
        {
            set_status(FAILURE);
        }
    }

    //  returns the status to the client (Behavior Tree)
    void set_status(int status)
    {
        // Set The feedback and result of BT.action
        feedback_.status = status;
        result_.status = feedback_.status;
        // publish the feedback
        as_.publishFeedback(feedback_);
        // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
        as_.setSucceeded(result_);

        switch (status)  // Print for convenience
        {
        case SUCCESS:
            ROS_INFO("Condition %s Succeeded", ros::this_node::getName().c_str() );
            break;
        case FAILURE:
            ROS_INFO("Condition %s Failed", ros::this_node::getName().c_str() );
            break;
        default:
            break;
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "condition_ros");
    ROS_INFO(" Enum: %d", RUNNING);
    ROS_INFO(" condition Ready for Ticks");
    BTAction bt_action(ros::this_node::getName());
    ros::spin();
    return 0;
}