Add visual command arrow to sim visualization

The simulation needs a visual indicator (green arrow) showing the commanded direction/velocity of the robot. E.g. visual linear velocity commands (x,y) and angular velocity/heading commands.