ros中改进a*算法的代码

在ROS中使用A*算法进行路径规划的步骤如下：

1. 创建一个地图，用网格表示，每个网格的值表示该区域是否可以通过，或者代表该区域的代价。
2. 创建一个A*算法的类，包含起始节点和终止节点。
3. 实现A*算法：

a. 初始化起始节点和终止节点，将起始节点加入open列表中。

b. 从open列表中选出f值最小的节点，将其作为当前节点。

c. 如果当前节点是终止节点，结束算法。

d. 否则，将当前节点从open列表中删除，加入closed列表中。

e. 对当前节点的相邻节点进行遍历，对于每个相邻节点：

i. 如果该节点已经在closed列表中，忽略它。

ii. 如果该节点不在open列表中，将其加入open列表中，并且设置它的父节点为当前节点，计算该节点的f、g、h值。

iii. 如果该节点已经在open列表中，计算该节点的新的f值，如果新的f值比旧的f值更小，则更新该节点的f、g、h值以及父节点。

f. 重复步骤b到e，直到open列表为空或者找到终止节点。

4. 返回起始节点到终止节点的路径。

下面是一个基本的A*算法实现示例：

#include <ros/ros.h>
#include <vector>
#include <queue>

using namespace std;

// 定义节点数据结构
struct Node
{
    int x, y; // 节点的坐标
    int g; // 起点到该节点的代价
    int h; // 该节点到终点的估价
    int f; // f = g + h
    int parent; // 父节点的下标
    Node(int _x, int _y, int _g, int _h, int _f, int _parent)
    {
        x = _x;
        y = _y;
        g = _g;
        h = _h;
        f = _f;
        parent = _parent;
    }
};

// A*算法类
class AStar
{
    public:
        AStar(vector<vector<int>> _map, int _start_x, int _start_y, int _end_x, int _end_y)
        {
            map = _map;
            start_x = _start_x;
            start_y = _start_y;
            end_x = _end_x;
            end_y = _end_y;
        }
        vector<Node> getPath()
        {
            // 定义open列表和closed列表，以及起点和终点节点
            vector<Node> open_list, closed_list, path;
            Node start_node(start_x, start_y, 0, 0, 0, -1);
            Node end_node(end_x, end_y, 0, 0, 0, -1);
            open_list.push_back(start_node);

            // 定义移动的方向
            int dx[8] = {1, 0, -1, 0, 1, 1, -1, -1};
            int dy[8] = {0, 1, 0, -1, 1, -1, 1, -1};

            // 开始A*算法
            while (!open_list.empty())
            {
                // 从open列表中选出f值最小的节点，将其作为当前节点
                int current_index = 0;
                for (int i = 0; i < open_list.size(); i++)
                {
                    if (open_list[i].f < open_list[current_index].f)
                    {
                        current_index = i;
                    }
                }
                Node current_node = open_list[current_index];

                // 如果当前节点是终止节点，结束算法
                if (current_node.x == end_node.x && current_node.y == end_node.y)
                {
                    Node node = current_node;
                    while (node.parent != -1)
                    {
                        path.push_back(node);
                        node = closed_list[node.parent];
                    }
                    path.push_back(node);
                    return path;
                }

                // 否则，将当前节点从open列表中删除，加入closed列表中
                open_list.erase(open_list.begin() + current_index);
                closed_list.push_back(current_node);

                // 对当前节点的相邻节点进行遍历
                for (int i = 0; i < 8; i++)
                {
                    int next_x = current_node.x + dx[i];
                    int next_y = current_node.y + dy[i];

                    // 如果该相邻节点不在地图内，或者该节点为障碍物，或者该节点已经在closed列表中，忽略它
                    if (next_x < 0 || next_x >= map.size() || next_y < 0 || next_y >= map[0].size() || map[next_x][next_y] == 1)
                    {
                        continue;
                    }

                    // 计算相邻节点的f、g、h值
                    int g = current_node.g + 1;
                    int h = sqrt(pow(next_x - end_x, 2) + pow(next_y - end_y, 2));
                    int f = g + h;

                    // 如果该相邻节点不在open列表中，将其加入open列表中，并且设置它的父节点为当前节点
                    // 计算该节点的f、g、h值
                    // 否则，如果该相邻节点已经在open列表中，计算该节点的新的f值，如果新的f值比旧的f值更小，则更新该节点的f、g、h值以及父节点
                    int next_index = -1;
                    for (int j = 0; j < open_list.size(); j++)
                    {
                        if (open_list[j].x == next_x && open_list[j].y == next_y)
                        {
                            next_index = j;
                        }
                    }
                    if (next_index == -1)
                    {
                        Node next_node(next_x, next_y, g, h, f, closed_list.size() - 1);
                        open_list.push_back(next_node);
                    }
                    else
                    {
                        if (g + h < open_list[next_index].f)
                        {
                            open_list[next_index].g = g;
                            open_list[next_index].h = h;
                            open_list[next_index].f = f;
                            open_list[next_index].parent = closed_list.size() - 1;
                        }
                    }
                }
            }

            return path;
        }
    private:
        vector<vector<int>> map; // 地图
        int start_x, start_y; // 起点坐标
        int end_x, end_y; // 终点坐标
};

int main(int argc, char **argv)
{
    // 初始化ros节点
    ros::init(argc, argv, "a_star_node");
    ros::NodeHandle nh;

    // 定义地图
    vector<vector<int>> map = {{0, 0, 0, 0, 0},
                               {0, 0, 1, 0, 0},
                               {0, 0, 1, 0, 0},
                               {0, 0, 1, 0, 0},
                               {0, 0, 0, 0, 0}};

    // 定义起点和终点
    int start_x = 0, start_y = 0;
    int end_x = 4, end_y = 4;

    // 创建A*算法对象并求解路径
    AStar astar(map, start_x, start_y, end_x, end_y);
    vector<Node> path = astar.getPath();

    // 输出路径
    for (int i = path.size() - 1; i >= 0; i--)
    {
        ROS_INFO("(%d, %d)", path[i].x, path[i].y);
    }

    return 0;
}

在这个示例中，地图用一个二维数组表示，0表示可以通过的区域，1表示障碍物。起点和终点的坐标也在main函数中定义。在AStar类中，getPath函数返回起点到终点的路径，路径用一个vector<Node>表示，Node结构体包含节点的坐标、f、g、h值以及父节点的下标。

需要注意的是，在实际应用中，地图可能是一个真实的环境，需要使用传感器获取数据。此时，需要使用机器人操作系统（ROS）来进行数据获取和处理。在ROS中，可以使用激光雷达、摄像头等传感器来获取环境数据，然后使用机器人导航包（Navigation Stack）来进行路径规划。Navigation Stack中包含了多种路径规划算法，包括A*算法。如果需要更加高效的路径规划算法，可以使用D*算法、RRT算法等。