import rclpy
from rclpy.node import Node
from py_trees.behaviour import Behaviour
from py_trees.common import Status
from py_trees.composites import Sequence, Selector
from py_trees.trees import BehaviourTree
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient

class MoveBaseAction(Behaviour):
    def __init__(self, name, goal_pose):
        super(MoveBaseAction, self).__init__(name)
        self.goal_pose = goal_pose
        self.action_client = None
        self.goal_handle = None
        self.feedback = None
        self.result_future = None
    
    def setup(self, **kwargs):
        node = kwargs.get('node')
        self.action_client = ActionClient(node, NavigateToPose, 'navigate_to_pose')
        self.action_client.wait_for_server()
        return True
    
    def initialise(self):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = self.goal_pose
        
        self.goal_handle = self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback)
        
        self.result_future = self.goal_handle.add_done_callback(self.goal_response_callback)
    
    def update(self):
        if self.result_future.done():
            goal_status = self.goal_handle.result().status
            if goal_status == 4:  # SUCCEEDED
                return Status.SUCCESS
            else:
                return Status.FAILURE
        return Status.RUNNING
    
    def terminate(self, new_status):
        if self.action_client.is_server_ready() and self.goal_handle is not None:
            self.action_client.cancel_goal_async(self.goal_handle)
    
    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.logger.error('Goal rejected by server')
            return
        
        self.logger.info('Goal accepted by server, waiting for result')
        self.result_future = goal_handle.get_result_async()
        self.result_future.add_done_callback(self.get_result_callback)
    
    def get_result_callback(self, future):
        result = future.result().result
        self.logger.info(f'Navigation result: {result}')
    
    def feedback_callback(self, feedback_msg):
        self.feedback = feedback_msg.feedback
        # 处理反馈信息

class BehaviorTreeNode(Node):
    def __init__(self):
        super().__init__('behavior_tree_node')
        
        # 创建行为树
        self.create_behavior_tree()
        
        # 创建定时器，定期执行行为树
        self.timer = self.create_timer(0.1, self.execute_behavior_tree)
        
        self.get_logger().info('Behavior tree node initialized')
    
    def create_behavior_tree(self):
        # 定义目标位置
        goal_pose1 = ...  # 填充目标位姿
        goal_pose2 = ...
        
        # 创建行为
        move_to_kitchen = MoveBaseAction("Move to Kitchen", goal_pose1)
        move_to_living_room = MoveBaseAction("Move to Living Room", goal_pose2)
        
        # 创建序列
        sequence = Sequence("Main Sequence")
        sequence.add_children([move_to_kitchen, move_to_living_room])
        
        # 创建行为树
        self.tree = BehaviourTree(sequence)
        
        # 设置节点
        self.tree.setup(node=self)
    
    def execute_behavior_tree(self):
        # 执行行为树
        self.tree.tick()

def main(args=None):
    rclpy.init(args=args)
    node = BehaviorTreeNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()