用Ａ*算法求自主导航的最短路径

A*算法是基于图搜索的一种较快捷的寻路算法，下面是它的基本思路。
一.节点的数据结构。在每一个路径节点或潜在的节点（即地图的栅格）中，具有的数据如下：
1.最基本的坐标值，在这里我们选择用栅格二维数组的下标而不是笛卡尔坐标x、y。
2.父节点，它意味着路径的拓展过程中本节点是从其父节点延伸过来的。
3.在每一轮迭代查找时，该路径点的优劣，它包括F，G，H三个数值，G指的是从起点移动到当前栅格的移动代价，要注意的是必须是从它的父节点延伸过来的移动代价，也就是说在某一时刻，该节点的G值是唯一确定的；H值指的是从该节点到终点的移动代价，我们用曼哈顿方法来估算这个代价；而F值是G与H之和，代表经过该节点到终点的总代价。
二.设置一个openlist，它用来搜索下一次迭代时潜在的路径点目标，由于在栅格地图中的路径点是在栅格中紧挨着依次摸索，不会出现跳跃的情况，因此这个openlist是每个路径点周围的八个栅格组成的，当然这样肯定会有重复，我们接下来阐述重复部分的处理方法；另外，我们设置一个closelist，这是在计算路径时已被我们确定的栅格，它们在下一次寻找中不会被我们考虑。因此，在每一次迭代中，我们把当前点的周围进行探索，将是障碍物的栅格、已经在closelist里的栅格忽略掉，其他的纳入openlist中。
三.当计算开始，首先将起点分别放入openlist与closelist，计算起点周围的节点，纳入openlist，它们的父节点是起点，而此时起点在openlist中已经无作用，将其删去。遍历openlist并选择F值最小的节点作为当前节点，放入closelist中，同时在openlist里去掉该节点。
四.第三点和第四点的行动将会反复执行。参照第二点的规则，遍历openlist并选择F值最小的节点作为当前节点，放入closelist中，同时在openlist里去掉该节点。在每一次执行后，查看该节点的相邻节点，若不在openlist与closelist中，就放进openlist，并且把父节点设为当前节点。再查看已在openlist中的相邻节点，如果经过当前节点到这些节点的G值小于它们的原有G值，那么更新这些节点的G值并将它们的父节点设为当前节点。
五.经过反复迭代，如果把终点纳入到openlist，则计算结束。我们从终点开始依次查找父节点，直至起点，这样便可得到路径。

在实际应用中，我们将ros中的占用栅格地图做成二维数组，这样便可以定位每一个节点的坐标下标位置。至于用什么数据结构能最快地确定某节点在openlist中，我们可以采用map来查找，也就是说，openlist和closelist都是map。

具体代码如下：

namespace astar 
{

using namespace std;
struct PointCell
{
    int x, y;
    /* 由于stl中没有关于结构体大小的定义，我们在这里重写红黑树排序的规则 */
    bool operator < (const PointCell& another) const
    {
        /* 把x作为主键 */
        if(x < another.x)
        {
          return true;
        }
        else if(x == another.x)
        {
          return y < another.y;
        }
        return false;
    }
};

struct TNode
{
    PointCell cell;
    TNode* father;
    int F, G, H;
};



class Astar
{
public:
    Astar();
    ~Astar();
private:
    bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
    void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a);
    bool currentSearch();
    void checkNeighborCell(TNode* node, PointCell neighbor, int deltaX, int deltaY);
    void updateGCost(TNode* node, TNode* neighbor, int deltaX, int deltaY);
    bool goalReached();

    int costGFunction(TNode* current, int deltaX, int deltaY)
    {
        if(deltaX != 0 && deltaY != 0)
            return current->father->G + 14;
        else
            return current->father->G + 10;
    }

    int costHFunction(TNode* current)
    {
        return abs(current->cell.x - goal_node_->cell.x) + abs(current->cell.y - goal_node_->cell.y);
    }

    TNode* root_node_, *current_node_, *goal_node_;

    ros::Publisher plan_pub_;
    ros::Subscriber pose_sub_, goal_sub_, map_sub_;
    void initialPoseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
    void goalCB(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg);
    pair<PointCell, TNode*> temp_node_;
    std::map<PointCell, TNode*> open_list_, close_list_;
    std::map<PointCell, TNode*>::iterator it_;
    PointCell start_, goal_;
    double map_origin_x_, map_origin_y_;
    double map_resolution_;
    int map_size_x_, map_size_y_;
    std::vector<int> map_info_;
    geometry_msgs::PoseStamped goal_pose_;
    std::vector<geometry_msgs::PoseStamped> plan_;
};

} /* end of namespace */

主程序如下：

namespace astar
{

Astar::Astar()
{
    ros::NodeHandle nh;
    plan_pub_ = nh.advertise<nav_msgs::Path>("astar_path", 1);
    pose_sub_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, boost::bind(&Astar::initialPoseCB, this, _1));
    map_sub_ = nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&Astar::mapCB, this, _1));
    goal_sub_ = nh.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal", 1, boost::bind(&Astar::goalCB, this, _1));
    root_node_ = new TNode;
    root_node_->father = NULL;
    current_node_ = new TNode;
    open_list_.clear();
    close_list_.clear();
}

Astar::~Astar()
{
    for(it_ = open_list_.begin(); it_ != open_list_.end(); it_++)
    {
        delete it_->second;
    }
    for(it_ = close_list_.begin(); it_ != close_list_.end(); it_++)
    {
        delete it_->second;
    }
}

bool Astar::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan)
{
    if(currentSearch())
    {
        ROS_INFO("Plan found.");
        std::vector<geometry_msgs::PoseStamped> temp_plan;
        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = "map";
        pose.pose.position.x = goal_node_->cell.x * map_resolution_ + map_origin_x_;
        pose.pose.position.y = goal_node_->cell.y * map_resolution_ + map_origin_y_;
        pose.pose.orientation.w = 1;
        temp_plan.push_back(pose);
        TNode* father = goal_node_->father;
        while(father != NULL)
        {
            pose.header.frame_id = "map";
            pose.pose.position.x = father->cell.x * map_resolution_ + map_origin_x_;
            pose.pose.position.y = father->cell.y * map_resolution_ + map_origin_y_;
            pose.pose.orientation.w = 1;
            temp_plan.push_back(pose);
            father = father->father;
        }
        plan.clear();
        for(int i = temp_plan.size() - 1; i > -1; i--)
        {
            plan.push_back(temp_plan[i]);
        }

        publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
        return true;
    }
    return false;
}

void Astar::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a)
{
    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());

    if(!path.empty())
    {
        gui_path.header.frame_id = path[0].header.frame_id;
        gui_path.header.stamp = path[0].header.stamp;
    }

    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for(unsigned int i=0; i < path.size(); i++){
        gui_path.poses[i] = path[i];
    }

    plan_pub_.publish(gui_path);
}

bool Astar::goalReached()
{
    it_ = open_list_.find(goal_);
    if(it_ == open_list_.end())
        return false;
    else
        return true;
}

bool Astar::currentSearch()
{
    /* 停止条件是openlist为空，或者将终点纳入到openlist中 */
    while(open_list_.size() != 0)
    {
        /* 遍历openlist，找寻F值最小的节点，选择它为当前节点 */
        int min_cost = INT_MAX;
        for(it_ = open_list_.begin(); it_ != open_list_.end(); it_++)
        {
            if(it_->second->F < min_cost)
            {
                current_node_ = it_->second;
            }
        }
        /* 把当前节点放入closelist */
        temp_node_.first = current_node_->cell;
        temp_node_.second = current_node_;
        close_list_.insert(temp_node_);

        /* 将当前节点从openlist中剔除 */
        open_list_.erase(current_node_->cell);

        /* 检查当前节点的相邻节点 */
        PointCell up, up_right, right, down_right, down, down_left, left, up_left;

        /* up */
        checkNeighborCell(current_node_, up, 0, 1);
        /* upright */
        checkNeighborCell(current_node_, up_right, 1, 1);
        /* right */
        checkNeighborCell(current_node_, right, 1, 0);
        /* downright */
        checkNeighborCell(current_node_, down_right, 1, -1);
        /* down */
        checkNeighborCell(current_node_, down, 0, -1);
        /* downleft */
        checkNeighborCell(current_node_, down_left, -1, -1);
        /* left */
        checkNeighborCell(current_node_, left, -1, 0);
        /* upleft */
        checkNeighborCell(current_node_, up_left, -1, 1);

        if(goalReached())
        {
            return true;
        }
    }
    return false;
}

void Astar::checkNeighborCell(TNode *node, PointCell neighbor, int deltaX, int deltaY)
{
    /* 如果这个邻居节点已在openlist当中，那检查经过本节点会不会有更好的路径代价 */
    it_ = open_list_.find(neighbor);
    if(it_ == open_list_.end())
    {
        /* 如果这个邻居节点也不在closelist中 */
        it_ = close_list_.find(neighbor);
        if(it_ == close_list_.end())
        {
            /* 如果它也不是障碍物，那么就将其放入openlist */
            if(map_info_[neighbor.y * map_size_x_ + neighbor.x] == 0)
            {
                TNode* newNode;
                newNode = new TNode;
                neighbor.x = node->cell.x + deltaX;
                neighbor.y = node->cell.y + deltaY;
                newNode->cell = neighbor;
                newNode->father = node;
                temp_node_.first = neighbor;
                temp_node_.second = newNode;
                open_list_.insert(temp_node_);
            }
        }
    }
    else
    {
        updateGCost(node, open_list_.find(neighbor)->second, deltaX, deltaY);
    }
}

void Astar::updateGCost(TNode* node, TNode* neighbor, int deltaX, int deltaY)
{
    int newCost = node->G + costGFunction(node, deltaX, deltaY);
    if(neighbor->G > newCost)
    {
        neighbor->G = newCost;
        neighbor->father = node;
    }
}

void Astar::initialPoseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
    ROS_INFO("start received.");
    start_.x = (msg->pose.pose.position.x - map_origin_x_) / map_resolution_;
    start_.y = (msg->pose.pose.position.y - map_origin_y_) / map_resolution_;
    root_node_->cell = start_;
    /* The first time for searching neighbors */
    temp_node_.first = start_;
    temp_node_.second = root_node_;
    open_list_.insert(temp_node_);
    current_node_ = root_node_;
}

void Astar::mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    ROS_INFO("map received.");
    map_resolution_ = msg->info.resolution;
    map_size_x_ = msg->info.width;
    map_size_y_ = msg->info.height;
    map_origin_x_ = msg->info.origin.position.x;
    map_origin_y_ = msg->info.origin.position.y;
    map_info_.clear();
    for(int i = 0; i < msg->data.size(); i++)
    {
        if(msg->data[i] == 0)
        {
            map_info_.push_back(0);
        }
        else
        {
            map_info_.push_back(1);
        }
    }
}

void Astar::goalCB(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    ROS_INFO("goal received.");
    goal_.x = (msg->pose.position.x - map_origin_x_) / map_resolution_;
    goal_.y = (msg->pose.position.y - map_origin_y_) / map_resolution_;
    goal_node_->cell = goal_;
    goal_pose_.pose = msg->pose;
    makePlan(goal_pose_, plan_);
}

} /* end of namespace */

int main(int argc, char **argv)
{
    ros::init(argc, argv, "astar");
    astar::Astar astar_test;
    ros::spin();
}

 这样可能还有一点点毛病，明天完善一下～

当然，map中的key一定要有序，这个我在一开始写opertator＜的时候绕了很多弯路，这样就直接导致了map在find的时候找到错误的TNode，浪费了很多时间．另外就是，我原来为了省事，用遍历来找F值最小的TNode，其实用优先级队列实现就行了．