• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

室外低速自动导航车的设计(8)——A*算法在ROS上的移植

人工智能 麻辣小蘑菇 1790次浏览 0个评论

A*算法是一种Greedy算法,ROS中的navigation导航包中的global_planner中就能找到。

今天,我们从头入手,自己移植并完成关于A*算法在ROS上的移植和实现。

首先来说一下我们最后想实现的目的:

我们首先画一个200*200像素的“交通路段”

在开始我们的算法之前,首先感谢网友[一路向北]的开源A*算法,他MFC界面的源代码网址如下:

              点击打开链接

我移植好的代码地址如下:

A*算法的具体实现我们并不关心,我们的目的提取它的核心算法,然后加入ROS相关的接口,并进行封装。

首先,算法移植前,我们一定要关注A*的核心类:

先看“一路向北”的原始代码,在这里由于源代码太长,我只列出封装的类:

class AStartFindPath
{
public:
    AStartFindPath();
    virtual ~AStartFindPath(){};
    int GetPos(int &x,int &y);
    void FindDestinnation(OpenList* open,CloseList* close);
    OpenList* FindMinInOpen(OpenList* open);
    bool Insert2OpenList(OpenList* , int x, int y);
    bool IsInOpenList(OpenList*, int x, int y);
    bool IsInCloseList(OpenList*, int x, int y);
    void IsChangeParent(OpenList*, int x, int y);
    bool IsAviable(OpenList* , int x, int y);
    unsigned int DistanceManhattan(int d_x, int d_y, int x, int y);
    unsigned int steps;
    int startpoint_x;
    int startpoint_y;
    int endpoint_x;
    int endpoint_y;
    int m_height,m_width;
    double m_resolution;
    //Lists
    OpenList* openlist;
    CloseList* closelist ;
    int x,y,des_x,des_y;
    char Thrs;
};

可以看到,这个AStarFindPath类有很多函数,但最主要的还是这个:

void FindDestinnation(OpenList* open,CloseList* close);

我们针对这个类做移植。 

首先,我们可以新建一个接口类,比如AStarInterface,一端调用AStarFindPath的函数,另一端连接ROS的标准协议。

我们也可以采用更为简单的方法,就是直接在AStarFindPath中添加ROS相关代码:

下面的代码为我修改后的代码:大家对照之前的代码,看加了一些什么内容。

class AStartFindPath
{
public:
    ros::NodeHandle n;
    Node **m_node;
    AStartFindPath();
    virtual ~AStartFindPath(){};
    int GetPos(int &x,int &y);
    void FindDestinnation(OpenList* open,CloseList* close);
    OpenList* FindMinInOpen(OpenList* open);
    bool Insert2OpenList(OpenList* , int x, int y);
    bool IsInOpenList(OpenList*, int x, int y);
    bool IsInCloseList(OpenList*, int x, int y);
    void IsChangeParent(OpenList*, int x, int y);
    bool IsAviable(OpenList* , int x, int y);
    unsigned int DistanceManhattan(int d_x, int d_y, int x, int y);
    /*以下是加入的ROS接口代码*/
    void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
    void start_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void end_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    ros::Subscriber map_sub;
    ros::Subscriber start_sub;
    ros::Subscriber end_sub;
    ros::Publisher nav_plan;
    //TF Scalar Listener
    tf::TransformListener AGV_transform_listener;
    tf::StampedTransform AGV_transform;
private:
    unsigned int steps;
    int startpoint_x;
    int startpoint_y;
    int endpoint_x;
    int endpoint_y;
    int m_height,m_width;
    double m_resolution;
    //Lists
    OpenList* openlist;
    CloseList* closelist ;
    int x,y,des_x,des_y;
    char Thrs;
    ros::Publisher map_pub;
};

我在一个部分前加了注释,表明那个类是我移植后加入的ROS相关,其中添加了3个函数:

void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void start_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void end_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg);

这种类型的函数通常被成为回调函数,回调函数是ROS收信息的接口;比如map_Callback就是收地图信息的函数,start_Callback是收到用户目标点激发的函数等等。

另外,在类的末尾,我加入了一个这个东西:ros::Publisher map_pub;这个Publisher叫消息发送器,消息发送器是ROS发信息的接口。现在我们在原来的类中加入了ROS的消息收发接口。 

既然我们定义好了接口,那就要对其实现。首先是地图的收取接口

void AStartFindPath::map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
    nav_msgs::OccupancyGrid m_map;
    m_height=msg->info.height;
    m_width=msg->info.width;
    m_resolution=msg->info.resolution;
    m_map.info.height=msg->info.height;
    m_map.info.width=msg->info.width;
    m_map.info.resolution=msg->info.resolution;
    if(m_node!=NULL)
    {
        for(int i=0;i<m_height;i++)delete [] m_node[i];
        delete [] m_node;
        m_node=NULL;
    }
 
    m_node=new Node*[m_height];
    for(int i=0;i    {
        m_node[i]=new Node[m_width];
        for(int j=0;j<m_width;j++)
        {
            m_node[i][j].location_x = j;
            m_node[i][j].location_y =i;
            m_node[i][j].parent = NULL;
            m_node[i][j].gray_val=msg->data[(i)*m_width+j];
            if(msg->data[(i)*m_width+j]!=0)m_node[i][j].flag = WALL;
            else m_node[i][j].flag = VIABLE;
 
        }
    }
    m_map.data.resize(m_height*m_width);
    for(int i=0;i<m_height;i++)
        for(int j=0;j<m_width;j++)
            m_map.data[i*m_width +j]=m_node[i][j].gray_val;
    map_pub.publish(m_map);
    printf("收到地图:长:%d Pixels----宽 %d Pixels\n",m_height,m_width);
}

  这个代码后边会再给大家讲,总之,申明了一个接口,一定要实现就可以了。

按照上述方案,把多个接口都完成,就可以参考我的另一篇文章点击打开链接对程序进行编译。

我完成的代码可以从下面这个网址下载:

点击打开链接

把代码复制到工作空间内可以直接编译,如果编译过程中有问题,可以在评论区留言。

关于代码的使用大家可以自己根据代码思考,后续我会再发布相关技术博客。


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明室外低速自动导航车的设计(8)——A*算法在ROS上的移植
喜欢 (0)

您必须 登录 才能发表评论!

加载中……