全局路径不正常的规划失败

所用全局路径的参数

1
2
3
4
5
6
planner_window_x: 0.0
planner_window_y: 0.0
default_tolerance: 0.0

publish_scale: 100
planner_costmap_publish_frequency: 0.0

全局路径很奇怪.png
全局路径失败时的报错.png
全局路径规划失败

看代码GlobalPlanner::makePlan中的if (found_legal)部分,再看GlobalPlanner::getPlanFromPotential函数,继续定位到path_maker_->getPath。这里就需要看path_maker是什么东西,在文件中发现

1
2
3
4
5
6
if (use_grid_path)   // 参数
//栅格路径,从终点开始找上下或左右4个中最小的栅格直到起点
path_maker_ = new GridPath(p_calc_);
else
//梯度路径,从周围八个栅格中找到下降梯度最大的点
path_maker_ = new GradientPath(p_calc_);

use_grid_path默认是false,所以一般用GradientPath,最终看GradientPath::getPath为什么返回false

if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) 中的0.5改为1

查来查去,终于发现其实就是全局代价地图里,本来就处于障碍的附近,所以规划如此奇怪。只是换到了室外环境,不习惯看全局代价地图了