修改官方包yocs_velocity_smoother 使其支持holonomic底盘，可以优化x, y, w速度。
参考同仁的文章https://www.jianshu.com/p/926d6e68ebd1
最初想法：
读yocs_velocity_smoother源码，理解其速度优化的方法，增加对y方向直线速度的优化。（感觉应该不难）
结果发现一下这一堆：
double MA = sqrt( v_inc * v_inc + w_inc * w_inc);
double MB = sqrt(max_v_inc * max_v_inc + max_w_inc * max_w_inc);
double Av = std::abs(v_inc) / MA;
double Aw = std::abs(w_inc) / MA;
double Bv = max_v_inc / MB;
double Bw = max_w_inc / MB;
double theta = atan2(Bw, Bv) - atan2(Aw, Av);
if (theta < 0)
{
// overconstrain linear velocity
max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);
}
else
{
// overconstrain angular velocity
max_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc);
}
if (std::abs(v_inc) > max_v_inc)
{
// we must limit linear velocity
cmd_vel->linear.x = last_cmd_vel.linear.x + sign(v_inc)*max_v_inc;
}
if (std::abs(w_inc) > max_w_inc)
{
// we must limit angular velocity
cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc;
}
告别数学多年，看到这里果断放弃了念头。
偷懒的方法：
源码中并不是简单的对x直线速度和w旋转速度分别进行某种计算，而是把这两种速度绑定在一起进行优化（x和w速度根本上是相互间有影响的）。
holonomic底盘，直线速度虽然涉及x和y两个方向，但实际结果为机器人向某一个方向直线移动（vx和vy的向量合成方向）。所以，我们可以把这个合成后的速度作为需要优化的直线速度， 这样就不要研究源码中的那一坨了。得到优化速度后再将其分解到x, y方向上。
代码实现
https://github.com/githubdanz/holonomic_velocity_smoother
编译问题请参考上一篇
