描述

关于A_算法的一些实现，我已经写在了我的博客里。这里也贴出链接来，大家可以去看
[移动机器人路径规划算法及思考——A_算法](https://blog.75271.com/43477.html “移动机器人路径规划算法及思考——A*算法”)

这篇我主要使用C++来实现了一下，发在这里，有一些代码写的还是不太优，以后会花些时间来优化的。

代码

代码分为三个文件，main.cpp，AStar.h头文件和AStar.cpp方法实现

我直接贴在下面

main.cpp

#include "AStar.h"
#include <iostream>

int main(int argc, char** argv)
{
    AStar astar;
    astar.set_mapPath("/Users/admin/Desktop/CodeBase/c++_try/map.jpeg");
    astar.set_startPoint(267, 281);
    astar.set_endPoint(968, 285);

    astar.search_path();

    return 0;
}

AStar.h

#ifndef __ASTAR_H__
#define __ASTAR_H__
#include <iostream>
#include <vector>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <string>
#include "stdlib.h"

typedef struct Node
{
    int x, y;
    int f, g, h;
    Node *father;
    Node(int x, int y)
    {
        this->x = x;
        this->y = y;
        this->f = 0;
        this->g = 0;
        this->h = 0;
        this->father = NULL;
    };    
}Node;

class AStar
{
    public:
        AStar();
        ~AStar();

    public:
        void set_mapPath(std::string map_path);
        void set_startPoint(int x, int y);
        void set_endPoint(int x, int y);

        bool search_path();

        void add_around();
        void add_around_singleDirection(int x, int y);

        bool check_node_mapRange(int x, int y);
        bool check_node_free(int x, int y);

        double compute_g(int x, int y);
        double compute_h(int x, int y);

        int check_openList(int x, int y);
        int check_closedList(int x, int y);
        void sort_openList();
        void quickSort(int left, int right, std::vector<Node*>& arr);

        void draw_map();
        void draw_path(Node *node);
        void clear_memory();
        void draw_online();

        std::string m_map_path;
        cv::Mat m_map_whole_color;
        cv::Mat m_map_whole_gray;
        int m_map_row, m_map_col;

        Node* m_start_node;
        Node* m_end_node;
        Node* m_current_node;

        std::vector<Node*> m_open_list;
        std::vector<Node*> m_closed_list;

        double m_weight_x = 1;
        double m_weight_y = 1;
        double m_weight_diagonal = sqrt(2);

};

#endif

AStar.cpp

#include "AStar.h"

AStar::AStar()
{

}

AStar::~AStar()
{

}

void AStar::set_mapPath(std::string map_path)
{
    m_map_path = map_path;
    m_map_whole_color = cv::imread(m_map_path, cv::IMREAD_COLOR);
    m_map_whole_gray = cv::imread(m_map_path, cv::IMREAD_GRAYSCALE); 
    if( m_map_whole_gray.empty())                     
    {
        std::cout <<  "Could not open or find the image" << std::endl;
    }

    m_map_row = m_map_whole_gray.size().width;
    m_map_col = m_map_whole_gray.size().height;

}

void AStar::set_startPoint(int x, int y)
{
    m_start_node = new Node(x, y);
}

void AStar::set_endPoint(int x, int y)
{
    m_end_node = new Node(x, y);
}

bool AStar::search_path()
{
    if (m_start_node->x < 0 || m_start_node->x > m_map_row 
    || m_start_node->y < 0 || m_start_node->y >m_map_col
    || m_end_node->x < 0 || m_end_node->x > m_map_row 
    || m_end_node->y < 0 || m_end_node->y >m_map_col)
    {
        std::cout<<"start/end point is out of the map range"<<std::endl;
        return false;
    }

    m_current_node = m_start_node;
    m_open_list.push_back(m_start_node);

    while(m_open_list.size() > 0)
    {
        m_current_node = m_open_list[0];

        add_around();

        m_closed_list.push_back(m_current_node);
        m_open_list.erase(m_open_list.begin());

        sort_openList();

        // draw_online();

        if (m_current_node->x == m_end_node->x && m_current_node->y == m_end_node->y)
        {
            draw_path(m_current_node);
            clear_memory();

            std::cout<<"Find path successful"<<std::endl;
            return true;
        }
    }
    return false;
}

void AStar::add_around()
{
    add_around_singleDirection(m_current_node->x - 1, m_current_node->y - 1); // left up corner
    add_around_singleDirection(m_current_node->x    , m_current_node->y - 1); // up 
    add_around_singleDirection(m_current_node->x + 1, m_current_node->y - 1); // right up corner
    add_around_singleDirection(m_current_node->x - 1, m_current_node->y    ); // left 
    add_around_singleDirection(m_current_node->x + 1, m_current_node->y    ); // right
    add_around_singleDirection(m_current_node->x - 1, m_current_node->y + 1); // left down corner
    add_around_singleDirection(m_current_node->x    , m_current_node->y + 1); // down
    add_around_singleDirection(m_current_node->x + 1, m_current_node->y + 1); // right down corner
}

void AStar::add_around_singleDirection(int x, int y)
{    
    if (!check_node_mapRange(x, y))
        return ;
    if (!check_node_free(x, y))
        return ;
    if (check_closedList(x, y) != -1)
        return ;

    double g_new = m_current_node->g + compute_g(x, y);

    int id = check_openList(x, y);

    if (id != -1)
    {
        Node* node_old = m_open_list[id];
        if (g_new < node_old->g)
        {
            node_old->g = g_new;
            node_old->f = node_old->g + node_old->h;
            node_old->father = m_current_node;    
        }
    }
    else
    {
        Node* node_new = new Node(x, y);
        node_new->g = g_new;
        node_new->h = compute_h(x, y);
        node_new->f = node_new->g + node_new->h;
        node_new->father = m_current_node;
        m_open_list.push_back(node_new);
    }
}

int AStar::check_openList(int x, int y)
{
    int num = m_open_list.size();
    for (int i = 0 ; i < num; i++)
    {
        if (x == m_open_list[i]->x && y == m_open_list[i]->y)
            return i;
    }
    return -1;
}

int AStar::check_closedList(int x, int y)
{
    int num = m_closed_list.size();
    for (int i = 0 ; i < num; i++)
    {
        if (x == m_closed_list[i]->x && y == m_closed_list[i]->y)
            return i;
    }
    return -1;
}

bool AStar::check_node_mapRange(int x, int y)
{
    if ( x < 0 || x > m_map_row 
    || y < 0 || y > m_map_col)
    {
        return false;
    }
    return true;
}

bool AStar::check_node_free(int x, int y)
{
    int color = m_map_whole_gray.at<uchar>(y,x);
    if (color > 250)
        return true;
    else
        return false;
}

double AStar::compute_g(int x, int y)
{
    double g = 0;
    if (abs(x - m_current_node->x)+ abs(y - m_current_node->y) == 2)
    {
        g = m_weight_diagonal;
    }
    if (abs(x - m_current_node->x) == 0 )
    {
        g = m_weight_y; 
    }
    if (abs(y - m_current_node->y) == 0 )
    {
        g = m_weight_x;
    }
    return g;
}

double AStar::compute_h(int x, int y)
{
    double h = sqrt(pow(x - m_end_node->x, 2) + pow(y - m_end_node->y, 2));
    return h;
}


void AStar::sort_openList()
{
    std::vector<double> f_list;
    quickSort(0, m_open_list.size()-1, m_open_list);
}

void AStar::quickSort(int left, int right, std::vector<Node*>& arr)
{
    if(left >= right)
        return;
    int i, j;
    Node* base = arr[left];
    i = left, j = right;
    base = arr[left];
    while (i < j)
    {
        while (arr[j]->f >= base->f && i < j)
            j--;
        while (arr[i]->f <= base->f && i < j)
            i++;
        if(i < j)
        {
            Node* temp = arr[i];
            arr[i] = arr[j];
            arr[j] = temp;
        }
    }
    arr[left] = arr[i];
    arr[i] = base;
    quickSort(left, i - 1, arr);
    quickSort(i + 1, right, arr);
}


void AStar::draw_map()
{
    cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );
    cv::imshow( "Display window", m_map_whole_color );
    cv::waitKey(0);
}

void AStar::draw_path(Node *node)
{
    if (node->father != NULL)
    {
        cv::circle(m_map_whole_color, cv::Point((int)(node->x), (int)(node->y)), 1, cv::Scalar(0, 0, 255), -1);
        draw_path(node->father);    
    }
    else
    {
        cv::namedWindow( "planner monitor", cv::WINDOW_AUTOSIZE);
        cv::imshow( "planner monitor", m_map_whole_color);
        cv::waitKey(0);
    }
}

void AStar::draw_online()
{
    cv::Mat m_map_show = m_map_whole_gray.clone();

    for (int i = 0; i < m_open_list.size() - 1; i++)
    {
        cv::circle(m_map_show, cv::Point((int)(m_open_list[i]->x), (int)(m_open_list[i]->y)), 1, cv::Scalar(0, 0, 0), -1);
    }
    cv::circle(m_map_show, cv::Point((int)(m_open_list[0]->x), (int)(m_open_list[0]->y)), 3, cv::Scalar(0, 0, 255), -1);
    cv::Mat m_map_local = m_map_show(cv::Rect(m_open_list[0]->x - 200, m_open_list[0]->y - 200, 
        400, 400));

    cv::namedWindow( "planner monitor", cv::WINDOW_AUTOSIZE);
    cv::imshow( "planner monitor", m_map_local);
    cv::waitKey(50);

}

void AStar::clear_memory()
{

}

效果

代码中的图片是我在网上随便搜的一张SLAM建图建出来的2D地图

这是在代码中设置了起点和终点跑出来的效果

喜欢的话点赞并关注哦