Unable to pass int type values between nodes , using Behavior tree 4.X

[IshanBhatnagar14]

@facontidavide @HenrikNilssonCode

I have CheckZValuePositive.h Class , which has an output port named "cycle" which stores increasing value of int type cycle_

Also have another class CheckWireValue.h , which has an input port "input_cycle_" ,

Linking the port in main.cpp

I am facing problem in linking output port of one class to input port of another class.

#include <ros/ros.h>
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/behavior_tree.h"
// #include <behaviortree_cpp_v3/bt_factory.h>
// #include <behaviortree_cpp_v3/behavior_tree.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Int32.h>
// #include <thread>

class CheckZValuePositive60: public BT::SyncActionNode
{
public: 
    
    CheckZValuePositive60(const std::string& name, const BT::NodeConfig& config)
        : BT::SyncActionNode(name, config),cycle_(0), currentZValue(0.0)
    {
        
        
        zValueSubscriber_ = nh_.subscribe("imu_topic",10, &CheckZValuePositive60::zValueCallback, this);
        last_time_ = ros::Time::now();
    }
    ~ CheckZValuePositive60()
    {
        zValueSubscriber_.shutdown();

        
    }

    static BT::PortsList providedPorts()
    {
        return { BT::OutputPort<int>("cycle") };
    }
    
    virtual BT::NodeStatus tick() override
    {
        
        setOutput("cycle",cycle_);
        ros::spinOnce();
        return BT::NodeStatus::SUCCESS;

             }

    
private:
    ros::NodeHandle nh_;

    ros::Subscriber zValueSubscriber_;
    double currentZValue;
    int cycle_ ; // Initialize cycle_ outside of the class definition
    ros::Time last_time_;
    
     
void zValueCallback(const sensor_msgs::Imu::ConstPtr& msg)
     {
        
        currentZValue = msg->orientation.z;
        if (currentZValue >=60 && currentZValue <= 70)
        {
            ros::Time now = ros::Time::now();
            ros::Duration elapsed = now - last_time_;
            ROS_INFO("Time elapsed: %.2f" , elapsed.toSec());
            last_time_ = now;
            if (elapsed.toSec() > 4)
            {
                cycle_++;
            }
                
            ROS_INFO("Cycle: %d", cycle_);
        }
         
     }
};

#include <ros/ros.h>
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/behavior_tree.h"
// #include <behaviortree_cpp_v3/bt_factory.h>
// #include <behaviortree_cpp_v3/behavior_tree.h>
#include <std_msgs/Int32.h>

class CheckWireValue: public BT::SyncActionNode
{
public: 
    
    CheckWireValue(const std::string& name, const BT::NodeConfig& config)
        : BT::SyncActionNode(name, config),currentWireValue(0.0),mot_trig_(false),crawler_move_(false)
    {
        
        
        cycle_value_ = 0;
        wireValueSubscriber_ = nh_.subscribe("wire_topic",10, &CheckWireValue::wireValueCallback, this);
        

    }
    
    ~ CheckWireValue()
    {
        wireValueSubscriber_.shutdown();

        
    }

    static BT::PortsList providedPorts()
    {
        return { BT::InputPort<int>("input_cycle_"),BT::OutputPort<bool>("motor_trigger"),BT::OutputPort<bool>("crawler_move") };

    }
    
    virtual BT::NodeStatus tick() override
    {
        
        // BT::Expected<int> val = getInput<int>("input_cycle_");
        // if (!val) {
        //     throw BT::RuntimeError("missing required input [input_cycle_]: ", 
        //                            this->name());
        // }

        auto val = getInput<int>("input_cycle_");
         if( !val )
      {
        throw BT::RuntimeError("error reading port [input_cycle]:", val.error());
      }
        cycle_value_ = val.value();
        setOutput("motor_trigger", mot_trig_);
        setOutput("crawler_move", crawler_move_);
      
        ROS_INFO("cycle_value_ is %d", cycle_value_);
        ros::spinOnce();
        return BT::NodeStatus::SUCCESS;

    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber wireValueSubscriber_;
    int cycle_value_;
    double currentWireValue;
    ros::Time last_time_;
    bool mot_trig_ ;
    bool crawler_move_;
    

    void wireValueCallback(const std_msgs::Int32::ConstPtr& msg)
    {
        currentWireValue = msg->data;

       switch(cycle_value_)
       {
    case 1:
        if (currentWireValue >= 0.024 && currentWireValue <= 0.026)
        {
            mot_trig_ = true;
        }
        break;
    case 2:
        if (currentWireValue >= 0.048 && currentWireValue <= 0.052)
        {
            mot_trig_ = true;
        }
        break;
    case 3:
        if (currentWireValue >= 0.072 && currentWireValue <= 0.078)
        {
            mot_trig_ = true;
        }
        break;
    case 4:
        if (currentWireValue >= 0.096 && currentWireValue <= 0.104)
        {
            mot_trig_ = true;
        }
        break;
    case 5:
        if (currentWireValue >= 0.120 && currentWireValue <= 0.130)
        {
            mot_trig_ = true;
        }
        break;
    case 6:
        if (currentWireValue >= 0.144 && currentWireValue <= 0.156)
        {
            mot_trig_ = true;
        }
        break;
    case 7:
        if (currentWireValue >= 0.168 && currentWireValue <= 0.182)
        {
            mot_trig_ = true;
        }
        break;
    case 8:
        if (currentWireValue >= 0.192 && currentWireValue <= 0.208)
        {
            mot_trig_ = true;
            crawler_move_ = true;
        }
        break;
    default:
        mot_trig_ = false;
        break;
        }
    
    }
};

#include "CheckZValuePositive60.h"
#include "CheckWireValue.h"
// #include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
// #include <behaviortree_cpp_v3/behavior_tree.h>
// #include <behaviortree_cpp_v3/bt_factory.h>
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/loggers/bt_cout_logger.h"

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

    // CheckZValuePositive60 CheckZValuePositive60;
    ros::init(argc,argv,"test_bt");

    ros::NodeHandle nh;

    BT::BehaviorTreeFactory factory ;
    // Set up the node registry with all the nodes that we want to use in this tree

    factory.registerNodeType<CheckZValuePositive60>("CheckZValuePositive60");
    factory.registerNodeType<CheckWireValue>("CheckWireValue");

    //  std::string xml_text = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>"
    //                        "<root BTCPP_format=\"3\">"
    //                        "  <BehaviorTree ID=\"ReactiveBehaviorTree\">"    
    //                        "      <CheckZValuePositive60/>"
    //                        "      <CheckWireValue/>"  
    //                        "  </BehaviorTree>"
    //                        "</root>";

  std::string xml_text = R"(
    <?xml version="1.0" encoding="UTF-8"?>
    <root BTCPP_format="3">
    <BehaviorTree ID="ReactiveBehaviorTree">
    <ReactiveSequence>
    <CheckZValuePositive60 cycle="cycle_key"/>
    <CheckWireValue input_cycle_="cycle_key"/>
    </ReactiveSequence>
    </BehaviorTree>
    </root>
)";

    auto tree = factory.createTreeFromText(xml_text);
        // R"(
        //     root {
        //         "CheckZValuePositive60
        //     }
        // )"



    BT::NodeStatus status = BT::NodeStatus::SUCCESS;

    while(ros::ok() && status == BT::NodeStatus::SUCCESS) {
        status = tree.rootNode()->executeTick();
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    

    

    return 0;
}

[facontidavide]

Because the XML is wrong. This is the correct one

<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="3">
  <BehaviorTree ID="ReactiveBehaviorTree">
     <ReactiveSequence>
       <CheckZValuePositive60 cycle="{cycle_key}"/>
       <CheckWireValue input_cycle_="{cycle_key}"/>
    </ReactiveSequence>
  </BehaviorTree>
</root>