base controller for mecanum wheels holonomic robot with cmd_vel

edit

I have a four mecanum wheels robot (I built it from scratch) and, at the moment, I'm driving it through a script installed on my motor controller, it is something like this:

' VAR 1 contains motion speed, +/-1000 range
' VAR 2 contains motion direction, 0-360 degree range
' VAR 3 contains rotation speed, +/-1000 range
VD = getvalue(_VAR, 1)
ThetaD = getvalue(_VAR, 2)
VTheta = getvalue(_VAR, 3)

if (VD <> PrevVD or ThetaD <> PrevThetaD or VTheta <> PrevVTheta)

         ThetaD45 = ThetaD + 45 ' compute once angle + 45 for use in the 4 equations        
         V1 = (VD * sin(ThetaD45))/1000 + VTheta ' sin takes degrees and returns result * 1000
         V2 = (VD * cos(ThetaD45))/1000 - VTheta
         V3 = (VD * cos(ThetaD45))/1000 + VTheta
         V4 = (VD * sin(ThetaD45))/1000 - VTheta

end if

PrevVD = VD
PrevThetaD = ThetaD
PrevVTheta = VTheta

where V1..V4 are the velocity for each motor.

Now, I would like to write a ROS node which subscribe to cmd_vel and execute motor commands for each motor. The problem is that the cmd_vel produced by move_base only publishes linear.x and angular.z in this way:

linear: 
  x: 0.8
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 1.68421052632

This is OK if you are using a non-holonomic robot, but since I'm on mecanum wheels, the robot is holonomic and so the linear.y value makes sense since it means lateral motion. Moreover, I need to know the ThetaD in order to make the above algorithm to work, should I have something like this?

linear_velocity_x_ = cmd.linear_x;
linear_velocity_y_ = cmd.linear_y;
angular_velocity_z_ = cmd.angular_z;

vel_dt_ = (current_time - last_vel_time_).toSec();
last_vel_time_ = current_time;

double delta_heading = angular_velocity_z_ * vel_dt_; //radians
double delta_x = (linear_velocity_x_ * cos(heading_) - linear_velocity_y_ * sin(heading_)) * vel_dt_; //m
double delta_y = (linear_velocity_x_ * sin(heading_) + linear_velocity_y_ * cos(heading_)) * vel_dt_; //m

In this case, double delta_heading should give me the ThetaAngle.

Or should I use something like:

 wheel_front_left = (1/WHEEL_RADIUS) * (linear.x – linear.y – (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angular.z);

wheel_front_right = (1/WHEEL_RADIUS) * (linear.x + linear.y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angular.z);

wheel_rear_left = (1/WHEEL_RADIUS) * (linear.x + linear.y – (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angular.z);

wheel_rear_right = (1/WHEEL_RADIUS) * (linear.x – linear.y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*angular.z);

In both cases, however, I would not have the linear.y value!!

Note: this question is related to my initial question 346720 about the use of move_base with a mecanum wheels robot.

P.S. Obviously, I set the configuration for holonomic output in the local_planner:

TrajectoryPlannerROS:
  max_vel_x: 1
  min_vel_x: 0.2
  max_vel_theta: 2.0
  min_in_place_vel_theta: 0.4

  acc_lim_theta: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5

  holonomic_robot: true
  y_vels: [-0.6, -0.1, 0.1, 0.6]

But it seems the planner doesn't think that y displacements would be OK and so it doesn't publish them.