ros中写一个发布一条由四个点确定一条贝塞尔曲线的python代码

以下是一个发布一条由四个点确定一条贝塞尔曲线的Python代码示例：

import rospy
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
from nav_msgs.msg import Path
from scipy.special import comb
import numpy as np

def publish_bezier_curve(path_pub, points):
    # 计算贝塞尔曲线的控制点
    control_points = []
    for t in np.linspace(0, 1, len(points)):
        p = np.zeros(2)
        for i, point in enumerate(points):
            p += comb(len(points)-1, i) * (1-t)**(len(points)-1-i) * t**i * np.array([point.x, point.y])
        control_points.append(Point(p[0], p[1], 0))

    # 发布路径消息
    path = Path()
    path.header.frame_id = "map"
    for point in control_points:
        pose = Pose()
        pose.position = point
        path.poses.append(pose)
    path_pub.publish(path)

if __name__ == '__main__':
    rospy.init_node('bezier_curve_publisher')
    path_pub = rospy.Publisher('/path', Path, queue_size=10)
    points = [Point(0, 0, 0), Point(1, 1, 0), Point(2, -1, 0), Point(3, 0, 0)]
    publish_bezier_curve(path_pub, points)
    rospy.spin()

希望这个代码能够帮助你！