# base controller for mecanum wheels holonomic robot with cmd_vel

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.