BT.py

#necessary imports here , follow basic Ros2 node stuff
import rclpy 
from rclpy.node import Node
from rclpy.action import ActionClient
import numpy as np
import py_trees
from py_trees.blackboard import Blackboard
import math

from irob_interfaces.srv import Activate, Deactivate, GetGoal, AtGoal 
from tf2_ros import Buffer, TransformListener

from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from action_msgs.msg import GoalStatus
from nav_msgs.msg import OccupancyGrid
from std_msgs.msg import Bool
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Twist


# --- LEAF BEHAVIOURS ---

class IsActive(py_trees.behaviour.Behaviour):
    def __init__(self, node: Node):
        super().__init__("IsActive")
        self.node = node
        self.active = None
        self.sub = node.create_subscription(
            Bool,
            "/robot_active",
            self.active_callback,
            10
        )
         

    def active_callback(self, msg):
        self.active = msg.data

    def update(self):
        print(f"IsActive", flush=True)
        if self.active is None:
            return py_trees.common.Status.RUNNING
        elif self.active:
            return py_trees.common.Status.SUCCESS
        return py_trees.common.Status.FAILURE

class Activation(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("Activation")
        self.flag = False 
        self.node = node 
        self._activate_client = self.node.create_client(Activate, 'activate')
        self.activate_flag = False 
    def initialise(self):
        self.flag = False
        self.activate_flag = False 
        print("Activation initialise", flush=True)

    def activate_request(self):
        print("Activate request", flush=True)
        request = Activate.Request()  
        future = self._activate_client.call_async(request)
        future.add_done_callback(self.activate_callback) 

    def activate_callback(self, future):
        response = future.result()
        if response.success:
            self.flag = True 
            self.activate_flag = False 
         
    def update(self):
        print(f"Activation", flush=True)
        if not self.activate_flag:
            result = self.activate_request()
            self.activate_flag = True

        if self.flag == False:
            return py_trees.common.Status.RUNNING
        elif self.flag == True:
            self.flag = False 
            return py_trees.common.Status.SUCCESS
        
class NewGoal(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("NewGoal")
        self.node = node
        self.blackboard = self.node.blackboard
        self.get_current_goal = self.node.create_subscription(PointStamped, '/goal_point', self.get_current_goal_callback, 10)
        self.published_goal = None

    def get_current_goal_callback(self, message):
        print("NewGoal: Current_goal callbak.", flush=True)
        self.published_goal = message.point

    def update(self):
        print("NewGoal", flush=True)

        self.goal = getattr(self.blackboard, "current_goal", None)
        print(f"NewGoal: Published goal", self.published_goal)
    
        if self.published_goal is None and self.goal is not None:
            print("We did not received a message from \goal_point", flush=True)
            return py_trees.common.Status.RUNNING
        
        elif self.published_goal is None and self.goal is None:
            print("We did not received a message from \goal_point but we have a goal ", flush=True)
            return py_trees.common.Status.FAILURE

        if self.goal is None:
            print("NewGoal: We don't have a goal yet.", flush=True)
            return py_trees.common.Status.FAILURE

        if self.goal[0] != self.published_goal.x or self.goal[1] != self.published_goal.y:
            print("NewGoal: New goal detected.", flush=True)
            detected_goal = (self.published_goal.x, self.published_goal.y)
            self.blackboard.current_goal = detected_goal
            return py_trees.common.Status.SUCCESS
        else:
            return py_trees.common.Status.FAILURE

class HaveGoal(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("HaveGoal")
        self.node = node
        self.blackboard = self.node.blackboard
        
    def update(self):
        print("HaveGoal", flush=True)
        self.goal_obs = getattr(self.blackboard, "goal_obs", None)
        # If this flag = True that means that the previous goal was an obstacle, we need a new one
        if self.goal_obs:
            print("The goal was an obstacle, we need a new one", flush=True)
            return py_trees.common.Status.FAILURE
        
        self.goal = getattr(self.blackboard, "current_goal", None)
        if self.goal is not None:
            print(f"The robot already has a goal.", flush=True)
            return py_trees.common.Status.SUCCESS 
        else:
            print("The robot has no goal.", flush=True)
            return py_trees.common.Status.FAILURE

class ExistGoal(py_trees.behaviour.Behaviour):
    def __init__(self, node: Node):
        super().__init__("ExistGoal")
        self.node = node
        self._goal_client = self.node.create_client(GetGoal, 'get_goal')
        self.previous_x = None
        self.previous_y = None
        self.goal_x = None
        self.goal_y = None
        self.request_sent = False  # ensure we only send one request per tick cycle
        self.blackboard = self.node.blackboard
    
    def initialise(self):
        self.request_sent = False
        self.previous_x = self.node.previous_goal[0]
        self.previous_y = self.node.previous_goal[1]

    def goal_request(self):
        print("GoalRequest sent")
        request = GetGoal.Request()
        future = self._goal_client.call_async(request)
        future.add_done_callback(self.getgoal_callback)
        self.request_sent = True

    def getgoal_callback(self, future):
        print("GetGoal Callback")
        response = future.result()
        if response is not None:
            self.goal_x = response.goal_x
            self.goal_y = response.goal_y
            print("goal x and y", self.goal_x, self.goal_y)
            

    def update(self):
        print("ExistGoal update", flush=True)
        
        if not self.request_sent:
            self.goal_request()
            print("sent a goal request")

        if self.goal_x is None or self.goal_y is None:
            return py_trees.common.Status.RUNNING

        if self.goal_x == self.previous_x and self.goal_y == self.previous_y:
            print("The new goal is equal to the old one, we can stop.", flush=True)
            return py_trees.common.Status.FAILURE
        else:
            print("We have a new goal.", flush=True)
            self.blackboard.current_goal = (self.goal_x, self.goal_y)
            self.node.previous_goal = (self.goal_x, self.goal_y)  
            return py_trees.common.Status.SUCCESS

class Deactivation(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("Deactivation")
        self.node = node
        self._deactivate_client = self.node.create_client(Deactivate, 'deactivate')
        self.request_sent = False 
        self.flag = False 
    
    def initialise(self):
        self.request_sent = False
        self.flag = False

    def deactivate_request(self):
        request = Deactivate.Request()
        future = self._deactivate_client.call_async(request)
        future.add_done_callback(self.deactivate_callback)
        self.request_sent = True


    def deactivate_callback(self, future):
        response = future.result()
        if response.success:
            self.flag = True

    def update(self):
        if not self.request_sent:
            self.deactivate_request()
        if self.flag:
            print(f"Deactivation completed.", flush=True)
            return py_trees.common.Status.SUCCESS 
        return py_trees.common.Status.RUNNING
        
class CheckGoal(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("CheckGoal")
        self.node = node
        self.subscription = self.node.create_subscription(OccupancyGrid, '/map', self.map_callback, 10)
        self.blackboard = self.node.blackboard
        self.tolerance = 13
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self.node)
        self.goal_obs = False 
    
    def initialise(self):
        self.goal_obs = False 
        self.blackboard.goal_obs = (self.goal_obs)

    def update(self):
        print("CheckGoal Update")
        
        self.goal = getattr(self.blackboard, "current_goal", None)

        if self.goal is None:
            print("No goal found in blackboard!")
            return py_trees.common.Status.FAILURE
        
        if not hasattr(self, "map_data"):
            print(f"CheckGoal has no map")
            return py_trees.common.Status.RUNNING
        
        print(f"Now I'll check the goal")

        grid = self.map_reshap()

        origin_x = self.map_data.info.origin.position.x
        origin_y = self.map_data.info.origin.position.y
        resolution = self.map_data.info.resolution

        grid_x = int((self.goal[0] - origin_x) / resolution)
        grid_y = int((self.goal[1] - origin_y) / resolution)

        if grid[grid_y][grid_x] != 0:
            print(f"The goal is an obstacle.", flush=True)
            self.goal_obs = True 
            self.blackboard.goal_obs = (self.goal_obs)
            return py_trees.common.Status.FAILURE
            
        else:
            print(f"The goal is free", flush=True)
            print("the value of the point is:", grid[grid_y][grid_x], flush=True)
            print("value of the goal but with switched index:", grid[grid_x][grid_y], flush=True) #wrong 
            return py_trees.common.Status.SUCCESS

    def map_callback(self, msg):
        self.map_data = msg
        print(f"A new map has been recived.", flush=True)
    
    def map_reshap(self):
        self.height = self.map_data.info.height 
        self.width = self.map_data.info.width   
        data_array = np.array(self.map_data.data, dtype=np.int8)
        grid = data_array.reshape((self.height, self.width))

        return grid 

class GoToGoal(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node, move_client):
        super().__init__("GoToGoal")
        self.node = node
        self._move_client = move_client
        self._at_goal_client = self.node.create_client(AtGoal, 'atgoal')
        self.goal_sent = False 
        self.nav_flag = False
        self.at_goal_flag = False  
        self.at_goal_request_sent = False
        self.current_goal_handle = None
        self.blackboard = self.node.blackboard  

    def initialise(self):
        self.goal_sent = False 
        self.nav_flag = False 
        self.at_goal_flag = False
        self.at_goal_request_sent = False
        self.current_goal_handle = None  

    def update(self):
        self.blackboard.goal_sent = self.goal_sent
    
        if not self.goal_sent:
            self.goal = getattr(self.blackboard, "current_goal", None)
            self.navigate_to(self.goal[0], self.goal[1])
            print(f"GoToGoal: Navigation request has been sent at the point.AAAAAAAAAAAAAAAAA", self.goal[0], self.goal[1], flush=True)
            return py_trees.common.Status.RUNNING
        
        if self.nav_flag:
            return py_trees.common.Status.SUCCESS

        if self.goal_sent and not self.at_goal_request_sent:
            self.at_goal_request()
            return py_trees.common.Status.RUNNING
        
        
        if self.at_goal_flag:
            self.at_goal_flag = False
            return py_trees.common.Status.SUCCESS

        return py_trees.common.Status.RUNNING
    
    def at_goal_request(self):
        request = AtGoal.Request()
        future = self._at_goal_client.call_async(request)
        future.add_done_callback(self.at_goal_callback)
        self.at_goal_request_sent = True
    
    def at_goal_callback(self, future):
        response = future.result()
        if response is not None:
            self.at_goal_flag = response.success 
            print(response.message)
            if not self.at_goal_flag:
                self.at_goal_request_sent = False#abort navigation

    def navigate_to(self, x, y):
       
        goal = PoseStamped()

        goal.header.frame_id = 'map'

        goal.pose.position.x = x 
        goal.pose.position.y = y 
        goal.pose.position.z = 0.0

        goal.pose.orientation.x = 0.0
        goal.pose.orientation.y = 0.0
        goal.pose.orientation.z = 0.0
        goal.pose.orientation.w = 1.0

        # now send the goal 
        nav_goal = NavigateToPose.Goal()
        nav_goal.pose = goal 
        self._move_client.wait_for_server()
        send_goal_future = self._move_client.send_goal_async(nav_goal)
        send_goal_future.add_done_callback(self.goal_response_callback)
        self.goal_sent = True

    def goal_response_callback(self, future):
        goal_handle = future.result()

        if not goal_handle.accepted:
            print(f"The goal was rejected", flush=True)
            return 
        else:
            print(f"The goal was accepted", flush=True)

        self.current_goal_handle = goal_handle
        self.blackboard.current_goal_handle = goal_handle
        _get_result_future = goal_handle.get_result_async()
        _get_result_future.add_done_callback(self.navigation_complete_callback)

    def navigation_complete_callback(self, future):
        result = future.result().status 

        if result == GoalStatus.STATUS_SUCCEEDED:
            self.nav_flag = True  
            print(f"Action completed", flush=True)
        
        else:
            print(f"Action failure", flush=True)


class StopNavigation(py_trees.behaviour.Behaviour):
    """
    Behaviour that cancels the active navigation goal from the blackboard.
    """

    def __init__(self, node:Node, move_client):
        super().__init__("StopNavigation")
        self.node = node
        self._move_client = move_client
        self.blackboard = self.node.blackboard
        self.cancel_future = None
        self.done = False

    def initialise(self):
        """
        Called when the behaviour is entered.
        """
        self.done = False

        # Retrieve the goal handle from the blackboard
        self.goal_handle = getattr(self.blackboard, "current_goal_handle", None)

        if self.goal_handle is None:
            self.node.get_logger().warn("StopNavigation: No active goal handle found on blackboard.")
            self.done = True
        else:
            # Send cancel request
            self.node.get_logger().info("StopNavigation: Cancelling current navigation goal...")
            self.cancel_future = self.goal_handle.cancel_goal_async()
            self.cancel_future.add_done_callback(self.cancel_done_callback)

    def update(self):
        """
        Called periodically while the behaviour is running.
        """

        print("StopNavigation: WE WANT TO STOP THAT", flush=True)

        if self.done:
            return py_trees.common.Status.SUCCESS
        else:
            return py_trees.common.Status.RUNNING

    def cancel_done_callback(self, future):
        """
        Callback triggered when the cancel request completes.
        """
        cancel_response = future.result()

        if len(cancel_response.goals_canceling) > 0:
            self.node.get_logger().info("StopNavigation: Navigation successfully cancelled.")
        else:
            self.node.get_logger().warn("StopNavigation: Failed to cancel navigation goal.")

        # Reset navigation state on the blackboard
        self.blackboard.goal_sent = False
        self.blackboard.nav_flag = False
        self.blackboard.at_goal_request_sent = False
        self.blackboard.current_goal_handle = None

        self.done = True


class IsLocalised(py_trees.behaviour.Behaviour):
    def __init__(self, node:Node):
        super().__init__("IsLocalised")
        self.node = node
        self.sub = self.node.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, 10)
        self.tolerance = 0.1
        self.blackboard = self.node.blackboard

    def update(self):
        if not hasattr(self, "cov"):
            print("No message from the topic /amcl_pose has been received.", flush=True)
            return py_trees.common.Status.RUNNING

        if self.cov[0] < self.tolerance and self.cov[7] < self.tolerance and self.cov[35] < self.tolerance:
            print("The robot is localised.", flush=True)
            print(self.cov[0], self.cov[7], self.cov[35])
            self.blackboard.is_localised = True
            return py_trees.common.Status.SUCCESS
        else:
            print("The robot is not localised. Localisation needed.", flush=True)
            self.blackboard.is_localised = False
            print(self.cov[0], self.cov[7], self.cov[35])
            return py_trees.common.Status.FAILURE

    def pose_callback(self, msg):
        self.cov = msg.pose.covariance
        print(f"We have recieved a message for the position", flush=True)

class Localisation(py_trees.behaviour.Behaviour):
    def __init__(self, node: Node):
        super().__init__("Localisation")
        self.node = node
        self.publisher_ = self.node.create_publisher(Twist, '/cmd_vel', 10)
        self.blackboard = self.node.blackboard

    def update(self):
        is_localised_flag = getattr(self.blackboard, "is_localised", False)
        if is_localised_flag:
            # Stop spinning
            msg = Twist()
            self.publisher_.publish(msg)
            return py_trees.common.Status.SUCCESS
        
        print("LOCALISATIONNNNNNNNNNNNNNN", flush=True)
        msg = Twist()
        # Spinning the robot
        msg.linear.x = 0.0
        msg.linear.y = 0.0
        msg.linear.z = 0.0
        msg.angular.x = 0.0
        msg.angular.y = 0.0
        msg.angular.z = 0.5
        self.publisher_.publish(msg)
        return py_trees.common.Status.RUNNING


class BTStudentsNode(Node):
    def __init__(self):
        super().__init__('BT_students_node')

        #add important initializations here
        self.get_logger().info("BT_students_node ready. Robot is inactive until activated.")
        self.move_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')


        self.current_goal = None
        self.previous_goal = (None, None)
        self.blackboard = Blackboard()

        self.root = self.create_tree()
        self.tree = py_trees.trees.BehaviourTree(self.root)
        self.timer = self.create_timer(1, self.tree.tick)

    

    def create_tree(self):
        root = py_trees.composites.Sequence("Root Sequence", memory=False)

        exist_goal = ExistGoal(self)
        
        get_goal_selector = py_trees.composites.Selector("Selector for aquiriing a new goal", memory=False)
        new_goal = NewGoal(self)
        have_goal = HaveGoal(self)
        deactivation = Deactivation(self)
        new_goal_sequence = py_trees.composites.Sequence("Sequence for checkig with new goal", memory=False)
        stop_navigation_2 = StopNavigation(self, self.move_client)
        new_goal_sequence.add_children([new_goal, stop_navigation_2])  
        get_goal_selector.add_children([
            new_goal_sequence,
            have_goal,
            exist_goal, 
            deactivation
        ])
        
        go_to_goal = GoToGoal(self, self.move_client)

        check_goal_selector = py_trees.composites.Selector("Selector for checking the goal", memory=False)

        check_goal = CheckGoal(self)
        stop_navigation = StopNavigation(self, self.move_client)

        check_goal_selector.add_children([
            check_goal,
            stop_navigation
        ])

        activation_selector = py_trees.composites.Selector("Selector for activating the goal", memory=False)
        is_active = IsActive(self)
        activation_leaf = Activation(self)
        activation_selector.add_children([is_active, activation_leaf])

        is_localised = IsLocalised(self)
        localisation = Localisation(self)
        stop_navigation3 = StopNavigation(self, self.move_client)

        localisation_selector = py_trees.composites.Selector("Selector for localisation", memory=False)
        localisation_sequence = py_trees.composites.Sequence("Selector for localisation", memory=False)

        localisation_sequence.add_children([
            stop_navigation3,
            localisation
        ])
        
        localisation_selector.add_children([
            is_localised, 
            localisation_sequence
        ])

        root.add_children([
            activation_selector, 
            localisation_selector,
            get_goal_selector,
            check_goal_selector, 
            go_to_goal
        ])
        return root


def main(args=None):
    rclpy.init(args=args)
    node = BTStudentsNode()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        print("Distruggo")
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()