帮我写一段切比雪夫做评估函函数、加入惩罚函数得A*算法的C++代码，要能实现的代码。不要伪代码

好的，以下是一个使用切比雪夫距离作为评估函数并加入惩罚函数的 A* 算法的 C++ 实现：

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
using namespace std;

// 定义地图节点类
class Node {
public:
    int x;
    int y;
    int g;   // 起点到当前节点的距离
    int h;   // 当前节点到终点的估计距离
    Node* parent;

    Node(int x, int y, int g = 0, int h = 0, Node* parent = nullptr)
        : x(x), y(y), g(g), h(h), parent(parent) {}

    int f() const { return g + h; }
};

// 计算切比雪夫距离
int chebyshev_distance(pair<int, int> a, pair<int, int> b) {
    return max(abs(a.first - b.first), abs(a.second - b.second));
}

// 定义惩罚函数
int penalty_function(Node* node, vector<pair<int, int>>& obstacles) {
    int penalty = 0;
    for (auto& obstacle : obstacles) {
        if (abs(node->x - obstacle.first) <= 1 && abs(node->y - obstacle.second) <= 1) {
            penalty += 100;
        }
    }
    return penalty;
}

// A*算法实现
vector<pair<int, int>> astar(pair<int, int> start, pair<int, int> end, vector<pair<int, int>>& obstacles) {
    priority_queue<pair<int, Node*>, vector<pair<int, Node*>>, greater<pair<int, Node*>>> open_list;
    vector<pair<int, int>> closed_list;
    Node* start_node = new Node(start.first, start.second, 0, chebyshev_distance(start, end) + penalty_function(new Node(start.first, start.second), obstacles));
    open_list.push(make_pair(start_node->f(), start_node));

    while (!open_list.empty()) {
        Node* current_node = open_list.top().second;
        open_list.pop();

        if (current_node->x == end.first && current_node->y == end.second) {
            vector<pair<int, int>> path;
            while (current_node) {
                path.push_back(make_pair(current_node->x, current_node->y));
                current_node = current_node->parent;
            }
            reverse(path.begin(), path.end());
            return path;
        }

        closed_list.push_back(make_pair(current_node->x, current_node->y));

        // 遍历当前节点的相邻节点
        for (int dx : {-1, 0, 1}) {
            for (int dy : {-1, 0, 1}) {
                int x = current_node->x + dx;
                int y = current_node->y + dy;

                // 判断节点是否在地图内
                if (x < 0 || x >= obstacles.size() || y < 0 || y >= obstacles[0].size()) {
                    continue;
                }

                // 判断节点是否为障碍物或者已经访问过
                if (find(obstacles.begin(), obstacles.end(), make_pair(x, y)) != obstacles.end()
                    || find(closed_list.begin(), closed_list.end(), make_pair(x, y)) != closed_list.end()) {
                    continue;
                }

                // 计算节点代价
                int g = current_node->g + 1;
                int h = chebyshev_distance(make_pair(x, y), end);
                int f = g + h + penalty_function(new Node(x, y), obstacles);
                Node* node = new Node(x, y, g, h, current_node);

                // 将节点加入开启列表
                open_list.push(make_pair(f, node));
            }
        }
    }

    return vector<pair<int, int>>();
}

int main() {
    vector<pair<int, int>> obstacles = { {1, 2}, {2, 2}, {3, 2}, {4, 2}, {5, 2}, {6, 2} };
    vector<pair<int, int>> path = astar(make_pair(0, 0), make_pair(9, 9), obstacles);

    cout << "Path: ";
    for (auto& p : path) {
        cout << "(" << p.first << ", " << p.second << ") ";
    }
    cout << endl;

    return 0;
}

需要注意的是，这只是一个简单的实现，如果需要用于实际场景中的路径规划，还需要考虑更多的因素，比如地形、障碍物的形状、大小等。