扩展yocs_velocity_smoother 支持holonomic 2018-01-08

修改官方包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

编译问题请参考上一篇