Waypoint Server using BehaviorTree

waypoint_server.cpp

#include <ros/ros.h>
#include <iostream>
#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>
#include <nav_msgs/Odometry.h>
#include <bits/stdc++.h>
#include <math.h>

using namespace BT;
std::pair<int,int> target (5, 5);
std::pair<int,int> current_coordinates;

namespace WaypointNodes {
    class WaypointReached : public BT::ConditionNode {
        WaypointReached(const std::string& name, const NodeConfiguration& config) : BT::ConditionNode(name, config) {

        }

        static PortsList providedPorts()
        {
            return { InputPort<std::string>("threshold_distance") };
        }

        BT::NodeStatus tick() override {

            Optional<std::string> msg = getInput<std::string>("threshold_distance");
            if(!msg) {
                ROS_INFO("%s", msg);
            }

            float distance = sqrt(pow(target.first - current_coordinates.first, 2) + pow(target.second - current_coordinates.second,    2));
            ROS_INFO("DISTANCE: %f", distance);
            if(distance < 2){
                ROS_INFO("WAYPOINT REACHED");
                return BT::NodeStatus::SUCCESS;
            }
            return BT::NodeStatus::RUNNING;
        }
    };

    class PublishNextWaypoint : public BT::SyncActionNode
    {
    public:
        PublishNextWaypoint(const std::string& name) : BT::SyncActionNode(name, {})
        {

        }

        BT::NodeStatus tick() override
        {
            ROS_INFO("Publishing next waypoint");
            target.first = 10;
            target.second = 10;
            return BT::NodeStatus::SUCCESS;
        }
    };

    BT::NodeStatus WaypointReachedSimple(BT::TreeNode& self);
    
    inline void RegisterNodes(BT::BehaviorTreeFactory& factory)
    {
        factory.registerNodeType<ConditionNode>("WaypointReached");
        factory.registerNodeType<PublishNextWaypoint>("PublishNextWaypoint");
    }
}

BT_REGISTER_NODES(factory) {
    WaypointNodes::RegisterNodes(factory);
} 

void odom_callback(const nav_msgs::Odometry::ConstPtr &msg) {
    current_coordinates.first = msg->pose.pose.position.x;
    current_coordinates.second = msg->pose.pose.position.y;
}

int main(int argc, char** argv) {
    using namespace WaypointNodes;
    BehaviorTreeFactory factory;

    factory.registerNodeType<PublishNextWaypoint>("PublishNextWaypoint");

    PortsList waypoint_reached_ports = { InputPort<std::string>("threshold_distance") };
    factory.registerSimpleAction("WaypointReached", WaypointReachedSimple, waypoint_reached_ports );

    auto tree = factory.createTreeFromFile("/home/sarath/ros2/catkin_ws/src/behavioral_waypoint_server/file.xml");
    
    ros::init(argc, argv, "behavioral_waypoint_server");
    ros::NodeHandle nh;
    ros::Subscriber odom_sub = nh.subscribe("odom", 1000, odom_callback);

    ros::Rate rate = 2;
    
    while(ros::ok) {
        tree.root_node->executeTick();
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}