f1tenth的多点导航+路径规划+pursuit追踪

一、发布起点，终点

pub_amcl:

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def publish_amcl_pose():
    # 初始化节点
    rospy.init_node('amcl_pose_publisher')

    # 创建发布者
    amcl_pose_pub = rospy.Publisher('/amcl_pose', PoseWithCovarianceStamped, queue_size=10)

    # 设置发布频率
    rate = rospy.Rate(1)  # 每秒发布一次，可根据需要调整

    while not rospy.is_shutdown():
        # 创建消息对象
        amcl_pose_msg = PoseWithCovarianceStamped()

        # 填充消息内容（示例数据，根据实际情况填充）
        amcl_pose_msg.header.stamp = rospy.Time.now()
        amcl_pose_msg.header.frame_id = 'map'  # 假设使用地图坐标系
        amcl_pose_msg.pose.pose.position.x =-13.473#-13.639#-13.457#-10.555#10.019#3.757#0.012 
        amcl_pose_msg.pose.pose.position.y =-0.116#-0.487#6.837#8.738#7.164#-0.148#2.318#-0.013 
        amcl_pose_msg.pose.pose.position.z &#