1.c语言编写路线
2.ROS中MPC局部路径规划器使用方法及源码流程解读
3.Navigation包 Global_planner全局路径规划源码详细解析

c语言编写路线

       #include <stdio.h>

       #include <malloc.h>

       #include<stdlib.h>

       #define MAX

       #define MAXNUM

       int previous[MAX-1];// 求路径需要

       int pp[MAX-1];// 记录最短路径

       typedef struct graphnode

       {

        int vexnum; //顶点

        int arcnum; //弧

        int gra[MAX][MAX]; //邻接矩阵表示0或1

       }Graph;

       int dist[MAX]; // 最短距离

       int arc[MAX][MAX]; // 权

       int main()

       {

        void Dijkstra(Graph *g,径路径int v);

        int i,j,n,m;

        int v; //源点

        Graph *G;

        G=(Graph *)malloc(sizeof(Graph));

        printf("vexnum:\n");

        scanf("%d",&G->vexnum);

        printf("arcnum:\n");

        scanf("%d",&G->arcnum);

        printf("graph:\n");

        for(i=0;i<G->vexnum;i++)

        for(j=0;j<G->vexnum;j++)

        {

        scanf("%d",&G->gra[i][j]);

        }

        for(i=0;i<G->vexnum;i++)

        for(j=0;j<G->vexnum;j++)

        {

        if(G->gra[i][j]==1)

        {

        printf("请输入%d到%d的权值:",i,j);

        scanf("%d",&arc[i][j]);//若有弧 则输入i到j直接的权

        }

        else

        arc[i][j]=MAXNUM;

        }

        printf("请输入源点v的值:");

        scanf("%d",&v);

        Dijkstra(G,v);

        printf("请输入源点所要到达的点：\n");

        scanf("%d",&n);

        pp[0]=0;

        i=1;

        m=n;// 记录n的值

        while(n!=0)// 求0到其他点路径

        {

        pp[i]=previous[n];

        i++;

        n=previous[n];

        }

        printf("Path:0 -> ");

        for(j=G->vexnum-1;j>=0;j--)

        if(pp[j]!=0)

        printf(" %d -> ",pp[j]);

        printf("%d\n",m);

        return 0;

       }

       void Dijkstra(Graph *G,int v)

       {

        int previous[MAX-1];

        int newdist;

        bool sign[MAX];

        if(v<0||v>MAX-1)

        {

        printf("该源点不存在！\n");

        return;

        }

        for(int i=0;i<G->vexnum;i++) //初始化

        {

        dist[i]=arc[v][i];

        sign[i]=false;

        if(dist[i]==MAXNUM)

        previous[i]=0;

        else

        previous[i]=v;

        }

        dist[v]=0;

        sign[v]=true;

        for(i=0;i<G->vexnum;i++) // i<n-1 待定

        {

        float temp=MAXNUM;

        int u=v; //u 中间变量

        for(int j=0;j<G->vexnum;j++)

        if((!sign[j])&&(dist[j]<temp))

        {

        u=j;

        temp=dist[j];

        }

        sign[u]=true;

        for(j=0;j<G->vexnum;j++)

        if((!sign[j])&&(arc[u][j]<MAXNUM))

        {

        newdist=dist[u]+arc[u][j];

        if(newdist<dist[j])

        {

        dist[j]=newdist;

        previous[j]=u;

        }

        }

        }

        for(i=0;i<G->vexnum;i++)

        if(dist[i]!=MAXNUM)

        printf("从%d到%d的最短路径是 %d\n",v,i,dist[i]);

        else

        printf("从%d到%d无最短路径\n",v,i);

        printf("\n");

       }

       è¿™æ˜¯Dijkstra算法求单源最短路径算法 上程序中 假定顶点从0开始，搜索整个图，然后求出0到其他各点的最短距离，存放在dist数组中，main函数后面几行是求0到其他各点的路径 基本上能满足你的要求了

ROS中MPC局部路径规划器使用方法及源码流程解读

       本文主要解析ROS Navigation框架中的MPC局部路径规划器mpc_local_planner的使用方法和源码流程。MPC模型预测控制算法是源码关键环节，它处理复杂环境，代码优化性能，径路径但计算复杂度较高。源码以下是代码spark 源码 sbtmpc_local_planner的详细步骤：

       1. 首先，将mpc_local_planner从GitHub或其他源代码库下载至ROS工作空间的径路径src文件夹。

       2. 环境配置需安装依赖和环境，源码可通过rosdep或参考相关博客解决安装问题。代码链接：[ROS Noetic版本 rosdep找不到命令 不能使用的径路径解决方法]。

       3. 通过catkin_make编译mpc_local_planner包，源码并通过其自带示例测试其功能，代码如阿克曼模型小车的径路径动态演示。

       4. 在move_base的源码launch文件中，将局部路径规划器设置为mpc_local_planner/MpcLocalPlannerROS，代码源码文件上传并根据机器人特性调整clearing_rotation_allowed参数，如阿克曼车型机器人禁止原地旋转。

       5. 配置参数文件mpc_local_planner_params.yaml，确保路径符合机器人实际情况。

       6. 完成配置后，进行实际路径规划测试，并根据测试结果调整参数，django view 源码以优化路径规划性能。

       以上步骤详尽介绍了在ROS中使用MPC局部路径规划器mpc_local_planner的步骤，通过这些操作，你将能更好地将其应用到你的机器人项目中。详情请参考《ROS中MPC局部路径规划器使用方法及源码流程解读》。

Navigation包 Global_planner全局路径规划源码详细解析

       学习总结，如有错误欢迎指正！道家网站源码

一丶plan_node.cpp

       从程序入口开始，首先在plan_node.cpp的main函数中，初始化了全局路径规划器。

costmap_2d::Costmap2DROS?lcr("costmap",?buffer);global_planner::PlannerWithCostmap?pppp("planner",?&lcr);

       在函数PlannerWithCostmap中设置了两种调用makePlan的路径：

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);}

       1.通过make_plan服务

req.start.header.frame_id?=?"map";req.goal.header.frame_id?=?"map";bool?success?=?makePlan(req.start,?req.goal,?path);

       2.通过goal回调函数

//得到当前机器人在MAP中的位置cmap_->getRobotPose(global_pose);makePlan(global_pose,?*goal,?path);

       在getRobotPose函数中，通过tf_.transform(robot_pose, global_pose, global_frame_);函数，默认将机器人pose从base_link转换到map坐标系下，可通过参数设置。flac源码1.3.2得到起始点和目标点传入到makePlan中。

二丶 planner_core.cpp//register?this?planner?as?a?BaseGlobalPlanner?pluginPLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner,?nav_core::BaseGlobalPlanner)

       global_planner 是基类nav_core :: BaseGlobalPlanner的一个插件子类

       首先在构造函数中需要初始化GlobalPlanner，在initialize中对一些参数进行赋值。

GlobalPlanner::GlobalPlanner(std::string?name,?costmap_2d::Costmap2D*?costmap,?std::string?frame_id)?:GlobalPlanner()?{ //initialize?the?plannerinitialize(name,?costmap,?frame_id);}

       当调用makePlan时，首先就是判断是否已经被初始化：

//?code?line?~?makePlan()if?(!initialized_)?{ ROS_ERROR("This?planner?has?not?been?initialized?yet,?but?it?is?being?used,?please?call?initialize()?before?use");return?false;}m

       初始化完成之后，清除之前规划的Plan，以防万一。然后检查起点和终点是否在我们所需要的坐标系下，一般在map系下。

//clear?the?plan,?just?in?case?，?code?line??makePlan()plan.clear();if?(goal.header.frame_id?!=?global_frame)?{ ...}if?(start.header.frame_id?!=?global_frame){ ...}

       将世界坐标系的点(map 坐标系)转换成图像坐标系(图像左下角)下的点(以像素表示)

if?(!costmap_->worldToMap(wx,?wy,?goal_x_i,?goal_y_i))?{ ROS_WARN_THROTTLE(1.0,"The?goal?sent?to?the?global?planner?is?off?the?global?costmap.?Planning?will?always?fail?to?this?goal.");return?false;}

       在Costmap2D和GlobalPlanner中都有实现worldToMap，其实都是一样的，在GlobalPlanner中则需要通过调用Costmap2D来获取局部地图的起始点和分辨率，而在Costmap2D则可以直接使用全局变量。

bool?Costmap2D::worldToMap(double?wx,?double?wy,?unsigned?int&?mx,?unsigned?int&?my)?const{ ?if?(wx?<?origin_x_?||?wy?<?origin_y_)return?false;?mx?=?(int)((wx?-?origin_x_)?/?resolution_);?my?=?(int)((wy?-?origin_y_)?/?resolution_);?if?(mx?<?size_x_?&&?my?<?size_y_)return?true;?return?false;}

       old_navfnbehavior ?作为一种旧式规划行为：

       The start of the path does not match the actual start location.

       The very end of the path moves along grid lines.

       All of the coordinates are slightly shifted by half a grid cell

       现在在worldToMap所使用的convert_offset_ = 0

       接下来将机器人Robot所在的位置，在costmap中设置成free，当前位置不可能是一个障碍物。 即在clearRobotCell()函数中：mx，my即当前机器人位置。

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);}0

       设置规划地图边框：outlineMap，此函数由参数outline_map_决定。 根据costmap跟起始终止点计算网格的potential，计算的算法有两种：Dijkstra和A*，具体算法便不再讨论，资料很多。 当提取到plan之后，调用getPlanFromPotential，把path转换变成geometry_msgs::PoseStamped数据类型。

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);}1

       此时便得到所需要的路径plan，最终调用OrientationFilter平滑之后发布出去。

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);}2