import rclpy
from rclpy.node import Node
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped
import time

class MultiNav(Node):
    def __init__(self):
        super().__init__('multi_nav')
        self.client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        self.goals = [
            [1.0, 0.0, 0.0],
            [1.0, 1.0, 1.57],
            [0.0, 1.0, 3.14],
            [0.0, 0.0, -1.57]
        ]

    def send_goal(self, x, y, yaw):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.orientation.z = yaw  
        self.client.wait_for_server()
        self.client.send_goal_async(goal_msg)

    def run(self):
        for x, y, yaw in self.goals:
            self.send_goal(x, y, yaw)
            time.sleep(10)  # 等待导航完成，可改为监听反馈

def main(args=None):
    rclpy.init(args=args)
    node = MultiNav()
    node.run()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()