A*学习和修改

A*算法修改权重

问题

由于项目需要，最近需要在A*中实现一种，当包含不平整路面时，如果不平整路面较短，则可以穿越，但是如果不平整路面很长，那就需要走平整的路面。因此需要修改不平整路面的权重。

思路

考虑使用Navigation包中的全局路径规划A*算法，在此基础上修改。
考虑将不平整路面设置为未知区域，即-1，但由于全局规划会不断更新路径，因此在前进中更新地图后，会更新全局规划。解决方案：代价地图中去除障碍物层，不考虑会有新的障碍参与进来。

Navigation 中的 A*的修改

在plan_core.cpp中，会选择全局规划器，是否使用gradient等参数。Note: 默认全局规划器是Dijkstra，非A*(太想当然了)
plan_core的思路是：首先使用A*或者Dijkstra等全局规划方法，计算出到终点的路径上的代价（calculatePotentials函数），然后再去得到路径的各个世界坐标(getPlanFromPotential函数)。

首先是对地图的一系列处理，可以不用太关注；

bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);
1
2

该函数是主要函数。
在astar.cpp中，添加周围节点的为函数add()，在其中会判度是否是已知，若是，就赋值为前一个节点的potential和当前节点的cost以及neutral_cost_的和，cost由于已知都为0，所以加不加无所谓。未知为-1，障碍物的直接跳过。neutral_cost_默认为50。

因此，如果未知区域，则仅需要对nertral_cost_加大权重即可。

同时，有个是使用gradient，还是grid的来getPath的。可能是因为未知时，cost为-1，所以如果用gradient会报错。因此，这个参数设置为grid,调用grid的getPath函数。

最后不同参数实现的效果。左上时W = 1，右上时W=1.2， 左下是W=1.5, 右下是W=5。

其中紫色是绝对障碍物，绿色、红色、棕色代表可以草坪、沙地等的可以路过，但是代价要比平滑路径的大。

当设置成5的时候，基本上就是看作障碍物了，此时路径长度554，计算耗时也最多，0.043。当为1.5的时候，只有最后一段草坪会穿过，路径为542，耗时0.029，当设置成1.2时，两个都会穿过。路径522,耗时0.014。当设置为一样的时候，即W=1，则三个都会穿过，路径为520，耗时0.005。和理论上一样。

可视化

这里为了将未知区域进行可视化，使用的是rviz的marker进行可视化。看的是师兄之前写过的代码以及网上的一些教程。
Marker
一个需要注意的是，在使用MarkerArray时，marker的id要进行赋值0，1，2……, 要不然只有第一个marker的会显示。但是，rviz的可视化，有亿点点的丑……

ENDING