Limit diagonal speed in path planning

[Flova]

Summary

Currently we can go full speed forwards and sideways at the same time. I think it would be reasonable to limit that based on the maximum forward/sideward movement. I don't know if a kite or ellipse shape for the maximum speed envelope would be suited best.

Is your feature request related to a problem?

Sometimes steps seem unreasonably large when we go diagonally. I observed this on the real robot at the integration test.

Describe the solution you'd like

Not only limit the movement speed along the x and y direction (while keeping the direction). We can also interpolate the max speed based on the angle.

Describe alternatives you've considered

Lower the x and y speed. We accept slower speeds.