移动机器人gazebo仿真(5)—规划算法Ａ*

最新推荐文章于 2025-07-10 10:25:28 发布

Musing_jk

最新推荐文章于 2025-07-10 10:25:28 发布

阅读量8.6k

点赞数 9

CC 4.0 BY-SA版权

分类专栏：移动机器人/ROS

本文链接：https://blog.youkuaiyun.com/weixin_37058227/article/details/79578154

本文深入探讨了ROS全局规划模块中的A*算法实现，从plan_node.cpp的makeplan函数开始，详细分析了astar.cpp中的关键算法步骤，包括calculatePotentials和add函数，讲解了A*算法如何计算潜在代价并找到最优路径。同时，提到了Dijkstra算法作为对比，并讨论了如何根据配置选择不同的路径规划算法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

参考博客：

MoveBace.cpp阅读笔记

贪心算法的一个典型案例——Dijkstra算法: 浅谈路径规划算法之Dijkstra算法

A*: A*寻路算法

关于寻路算法的一些思考（1）——A*算法介绍

ROS的全局规划代码介绍（A*）

重要：ROS Navigation的global_planner类继承关系与实现算法

导航实际流程为：

进行全局路径规划，在进行局部路径规划，然后发布速度

全局路径规划在makePlan函数中，该函数中调用了 planner_的 makePlan和 empty接口。
planner_为继承于BaseGlobalPlanner的实例，由pluginlib通过具体类的名字进行装载。
之后，调用tc_的setPlan接口，对局部路径规划器进行全局路径设置，然后，调用tc_的isReached接口进行判断，然后调用tc_的computeVelocityCommands接口，进行速度计算，然后进行速度下发。

tc_为继承于BaseLocalPlanner的实例，也是由pluginlinb通过具体类的名字进行装载。

下面带来两个问题，planner_怎么进行路径规划，以及tc_如何计算速度。
planner_在初始化时候，被塞入了planner_costmap_ros_
tc_在初始化时，被塞入了controller_costmap_ros_

在global planner的包中，注册了插件：global planner::GlobalPlanner

代码阅读：

global_planne

1、plan_node.cpp

plan_node.cpp是全局规划代码的入口(代码注释都是自己理解然后添加，也许会有错误)

#include <global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>

namespace cm = costmap_2d;
namespace rm = geometry_msgs;

using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;

namespace global_planner {

class PlannerWithCostmap : public GlobalPlanner {
    public:
        PlannerWithCostmap(string name, Costmap2DROS* cmap);
        bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);

    private:
        void poseCallback(const rm::PoseStamped::ConstPtr& goal);
        Costmap2DROS* cmap_;
        ros::ServiceServer make_plan_service_;
        ros::Subscriber pose_sub_;
};
//Publish a path for visualization purposes
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {
    vector<PoseStamped> path;

    req.start.header.frame_id = "/map";
    req.goal.header.frame_id = "/map";
    bool success = makePlan(req.start, req.goal, path);
    resp.plan_found = success;
    if (success) {
        resp.path = path;
    }

    return true;
}

void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {
    tf::Stamped<tf::Pose> global_pose;
    cmap_->getRobotPose(global_pose);//获取机器人起始位姿
    vector<PoseStamped> path;
    rm::PoseStamped start;
    start.header.stamp = global_pose.stamp_;
    start.header.frame_id = global_pose.frame_id_;
    start.pose.position.x = global_pose.getOrigin().x();
    start.pose.position.y = global_pose.getOrigin().y();
    start.pose.position.z = global_pose.getOrigin().z();
    start.pose.orientation.x = global_pose.getRotation().x();
    start.pose.orientation.y = global_pose.getRotation().y();
    start.pose.orientation.z = global_pose.getRotation().z();
    start.pose.orientation.w = global_pose.getRotation().w();
    makePlan(start, *goal, path);//路径规划
}

PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
        GlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
    ros::NodeHandle private_nh("~");
    cmap_ = cmap;
    make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
    pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}

} // namespace

int main(int argc, char** argv) {
    ros::init(argc, argv, "global_planner");
//设置tf监听时间间隔
    tf::TransformListener tf(ros::Duration(10));
//costmap_2d::Costmap2D 类是存储和访问2D代价地图的的基本数据结构，下面代码作用是初始化
    costmap_2d::Costmap2DROS