写一个ros的A*代码 c++

好的，以下是一个简单的 A* 算法的实现，使用ROS中的nav_msgs::OccupancyGrid作为地图输入，以及geometry_msgs::PoseStamped作为起点和终点输入。

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <queue>
#include <vector>
#include <cmath>

struct Node
{
    int x;
    int y;
    int f;
    int g;
    int h;
    Node* parent;

    Node(int x_, int y_, int f_, int g_, int h_, Node* parent_)
        :x(x_), y(y_), f(f_), g(g_), h(h_), parent(parent_){}
};

class AStarPlanner
{
public:
    AStarPlanner(ros::NodeHandle& nh)
        :nh_(nh), map_received_(false)
    {
        map_sub_ = nh_.subscribe("map", 1, &AStarPlanner::mapCallback, this);
        goal_sub_ = nh_.subscribe("goal", 1, &AStarPlanner::goalCallback, this);
        path_pub_ = nh_.advertise<nav_msgs::Path>("path", 1);
    }

    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
    {
        map_ = *msg;
        map_received_ = true;
    }

    void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    {
        goal_ = *msg;
        if(map_received_)
        {
            findPath();
        }
    }

    void findPath()
    {
        // 起点和终点的像素坐标
        int start_x = (map_.info.origin.position.x - goal_.pose.position.x) / map_.info.resolution;
        int start_y = (map_.info.origin.position.y - goal_.pose.position.y) / map_.info.resolution;
        int goal_x = (map_.info.origin.position.x - goal_.pose.position.x) / map_.info.resolution;
        int goal_y = (map_.info.origin.position.y - goal_.pose.position.y) / map_.info.resolution;

        // 节点列表，用于存放待扩展的节点
        std::vector<Node*> open_list;
        // 节点列表，用于存放已扩展的节点
        std::vector<Node*> closed_list;

        // 初始化起点节点
        Node* start_node = new Node(start_x, start_y, 0, 0, 0, nullptr);
        open_list.push_back(start_node);

        while(!open_list.empty())
        {
            // 从open_list中选取f值最小的节点
            int min_f = INT_MAX;
            int min_index = 0;
            for(int i = 0; i < open_list.size(); i++)
            {
                if(open_list[i]->f < min_f)
                {
                    min_f = open_list[i]->f;
                    min_index = i;
                }
            }
            Node* current_node = open_list[min_index];

            // 判断是否到达终点
            if(current_node->x == goal_x && current_node->y == goal_y)
            {
                // 生成路径并发布
                nav_msgs::Path path;
                Node* node = current_node;
                while(node)
                {
                    geometry_msgs::PoseStamped pose;
                    pose.pose.position.x = map_.info.origin.position.x - node->x * map_.info.resolution;
                    pose.pose.position.y = map_.info.origin.position.y - node->y * map_.info.resolution;
                    path.poses.push_back(pose);
                    node = node->parent;
                }
                std::reverse(path.poses.begin(), path.poses.end());
                path_pub_.publish(path);

                // 释放内存
                for(int i = 0; i < open_list.size(); i++)
                {
                    delete open_list[i];
                }
                for(int i = 0; i < closed_list.size(); i++)
                {
                    delete closed_list[i];
                }
                return;
            }

            // 将当前节点从open_list中移除，加入到closed_list中
            open_list.erase(open_list.begin() + min_index);
            closed_list.push_back(current_node);

            // 扩展当前节点的邻居节点
            for(int dx = -1; dx <= 1; dx++)
            {
                for(int dy = -1; dy <= 1; dy++)
                {
                    if(dx == 0 && dy == 0)
                        continue;

                    int x = current_node->x + dx;
                    int y = current_node->y + dy;

                    // 判断是否越界
                    if(x < 0 || x >= map_.info.width || y < 0 || y >= map_.info.height)
                        continue;

                    // 判断是否为障碍物
                    int index = y * map_.info.width + x;
                    if(map_.data[index] == 100)
                        continue;

                    // 判断节点是否已经在closed_list中
                    bool in_closed_list = false;
                    for(int i = 0; i < closed_list.size(); i++)
                    {
                        if(closed_list[i]->x == x && closed_list[i]->y == y)
                        {
                            in_closed_list = true;
                            break;
                        }
                    }
                    if(in_closed_list)
                        continue;

                    // 计算g值和h值
                    int g = current_node->g + sqrt(dx*dx + dy*dy);
                    int h = sqrt((x - goal_x)*(x - goal_x) + (y - goal_y)*(y - goal_y));

                    // 判断节点是否已经在open_list中
                    bool in_open_list = false;
                    for(int i = 0; i < open_list.size(); i++)
                    {
                        if(open_list[i]->x == x && open_list[i]->y == y)
                        {
                            in_open_list = true;
                            if(g < open_list[i]->g)
                            {
                                open_list[i]->g = g;
                                open_list[i]->f = g + open_list[i]->h;
                                open_list[i]->parent = current_node;
                            }
                            break;
                        }
                    }
                    if(!in_open_list)
                    {
                        Node* node = new Node(x, y, 0, g, h, current_node);
                        node->f = g + h;
                        open_list.push_back(node);
                    }
                }
            }
        }

        // 释放内存
        for(int i = 0; i < open_list.size(); i++)
        {
            delete open_list[i];
        }
        for(int i = 0; i < closed_list.size(); i++)
        {
            delete closed_list[i];
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber map_sub_;
    ros::Subscriber goal_sub_;
    ros::Publisher path_pub_;

    nav_msgs::OccupancyGrid map_;
    bool map_received_;

    geometry_msgs::PoseStamped goal_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "astar_planner");
    ros::NodeHandle nh;

    AStarPlanner planner(nh);

    ros::spin();

    return 0;
}

需要注意的是，这只是一个简单的实现，还有很多细节需要根据实际应用进行优化和改进。