代码链接:https://github.com/karlkurzer/path_planner
- Bug: 平滑后得到的姿态角在rviz上显示出错。
- 示例:箭头表示当前车体位姿,指向车头方向,红色、黑色分别代表起点、终点的位姿,蓝色为未经平滑,橙色经过平滑后的(短路径看不太出来效果,但是平滑有必要)。最离谱就是第三段,原地掉个儿,令人头秃。。。
- 解决方法:(1) atan2()返回值为(-pi,pi),需改为(0-2pi)(注意:0-pi不变,-pi-0改为pi-2pi),改变语句为
newPath[i - 1].setT(Helper::normalizeHeadingRad(std::atan2(Dxi.getY(), Dxi.getX())));
。
(2)未考虑车辆前进or后退,根据方向改变dxi,加入语句if(newPath[i].getPrim() <3) Dxi = -Dxi;
。
//###################################################
// SMOOTHING ALGORITHM
//###################################################
void Smoother::smoothPath(DynamicVoronoi& voronoi) {
// load the current voronoi diagram into the smoother
this->voronoi = voronoi;
this->width = voronoi.getSizeX();
this->height = voronoi.getSizeY();
// current number of iterations of the gradient descent smoother
int iterations = 0;
// the maximum iterations for the gd smoother