ROS中，用C++写一个pure pusuit 算法

以下是一个用C语言编写的Pure Pursuit算法的示例：

#include <stdio.h>
#include <math.h>

#define MAX_SPEED 5.0
#define MAX_STEERING_ANGLE 0.5
#define LOOKAHEAD_DISTANCE 1.0

typedef struct {
    double x;
    double y;
} Point;

double distance(Point a, Point b) {
    return sqrt(pow(a.x - b.x, 2.0) + pow(a.y - b.y, 2.0));
}

double angle(Point a, Point b) {
    return atan2(b.y - a.y, b.x - a.x);
}

double normalize_angle(double angle) {
    while (angle > M_PI) {
        angle -= 2.0 * M_PI;
    }
    while (angle < -M_PI) {
        angle += 2.0 * M_PI;
    }
    return angle;
}

double steering_angle(Point current_pos, double current_heading, Point target_pos) {
    double alpha = normalize_angle(angle(current_pos, target_pos) - current_heading);
    double L = distance(current_pos, target_pos);
    double steering_angle = atan2(2.0 * L * sin(alpha) / LOOKAHEAD_DISTANCE, 1.0);
    steering_angle = fmax(fmin(steering_angle, MAX_STEERING_ANGLE), -MAX_STEERING_ANGLE);
    return steering_angle;
}

double throttle(Point current_pos, double current_heading, Point target_pos) {
    double L = distance(current_pos, target_pos);
    double throttle = MAX_SPEED * (1.0 - exp(-L / LOOKAHEAD_DISTANCE));
    return throttle;
}

int main() {
    Point current_pos = {0.0, 0.0};
    double current_heading = 0.0;
    Point target_pos = {1.0, 1.0};

    double steer = steering_angle(current_pos, current_heading, target_pos);
    double throttle = throttle(current_pos, current_heading, target_pos);

    printf("Steering Angle: %f\n", steer);
    printf("Throttle: %f\n", throttle);

    return 0;
}

该示例代码中，定义了一个`Point`结构体表示二维坐标点，以及一些常量，如最大速度、最大转向角度、前瞻距离等。然后，定义了一些函数来计算两个点之间的距离、方向角度、规范化角度等。

接着，定义了两个主要函数，即`steering_angle()`和`throttle()`，分别用于计算当前车辆应该转向的角度和加速度。这两个函数中都使用了前面定义的一些常量和函数，并根据车辆当前位置和朝向，以及目标位置计算出相应的值。

最后，在`main()`函数中，定义了当前车辆位置、朝向和目标位置，然后调用`steering_angle()`和`throttle()`函数来计算车辆的转向角度和加速度，并输出结果。