实现nav_在ROS中实现A*路径规划

最新推荐文章于 2024-01-17 21:12:27 发布

瑞恩的奇幻博物馆

最新推荐文章于 2024-01-17 21:12:27 发布

阅读量1.9k

点赞数 4

文章标签：实现nav

本文链接：https://blog.csdn.net/weixin_29159613/article/details/112696625

版权

本文档详细介绍了如何在ROS中实现A*路径规划算法。从核心代码的编写，包括初始化、路径规划实现，到注册插件、调用插件及在RVIZ中的路径可视化，一步步阐述了整个过程。通过遵循导航包中的指导，开发者可以创建自己的路径规划插件并在ROS环境中应用。

摘要由CSDN通过智能技术生成

在ROS中实现自己的路径规划算法需要以plugin的方式，并且继承nav_core::BaseGlobalPlanner C++ 接口。

下面将逐步介绍如何实现A*算法在ROS中的路径规划。

1.核心代码

1.1 initialize

    void AstarPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    
        if(!initialized_){
    
            costmap_ros_ = costmap_ros;
            costmap_ = costmap_ros_->getCostmap();
            width = costmap_->getSizeInCellsX();
            height = costmap_->getSizeInCellsY();
            map_size = width * height;
            OGM.resize(map_size);

            for (int i = 0; i < width; i++)
            {
    
                for (int j = 0; j < height; j++)
                {
    
                    unsigned int cost = static_cast<int>(costmap_->getCost(j, i));
                    //get_cost << cost << endl;
                    //cout << "i:, j:" << cost << endl;
                    
                    if (cost == 0)
                        OGM[i * width + j] = true;
                    else
                        OGM[i * width + j] = false;
                    
                }
            }

            frame_id_ = costmap_ros->getGlobalFrameID();
            
            ros::NodeHandle private_nh("~/" + name);
            
            plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
            
            initialized_ = true;
        }
        else
            ROS_WARN("This planner has already been initialized... doing nothing");
    }

获取地图的高度（height）和宽度（width），将地图栅格化costmap-&