Change slotcar control to explicitly open loop

[luca-della-vedova]

Bug fix

Supersedes #40, closes #37

Fixed bug

Do explicit open loop control instead of relying on undefined behavior (that broke in the ignition-gazebo version 5.1).

Fix applied

We currently rely on reading the Linear / AngularVelocityCmd components to know what is the current robot's speed, this is equivalent to doing open loop control since the components used to have the command sent in the previous iteration.
However, it was actually an incorrect behavior since those components are supposed to be set to zero before the following simulation step, as is done here.
Switching to closed loop simulation as proposed in #40 doesn't work for dartsim because the high friction caused by our way of moving the robots (move it as a box, sliding it on the floor) causes the velocities to be very low and the robot to barely move, as discussed in ign-gazebo #966(comment).

For these reasons, the only way to have the robots to have the same behavior with both physics engines as before is to switch to an explicit open loop control, where we save the velocities of the previous step and use them to compute the updated velocities in the following step.

[codecov]

Codecov Report

Merging #43 (428946a) into main (fc7fb26) will not change coverage.
The diff coverage is n/a.

@@           Coverage Diff           @@
##            main     #43     +/-   ##
=======================================
  Coverage   0.00%   0.00%             
=======================================
  Files         72     138     +66     
  Lines       6490   12857   +6367     
=======================================
- Misses      6490   12857   +6367     

Flag	Coverage Δ
tests	0.00% <ø> (ø)

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...ing_sim_gazebo_plugins/src/thumbnail_generator.cpp
...ation/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp
...lation/rmf_building_sim_common/src/lift_common.cpp
...obot_sim_ignition_plugins/src/TeleportIngestor.cpp
...m_ignition_plugins/src/LightTuning/LightTuning.cpp
...f_simulation/rmf_building_sim_common/src/utils.cpp
...ation/rmf_building_sim_gazebo_plugins/src/lift.cpp
...uilding_sim_gazebo_plugins/src/crowd_simulator.hpp
...building_sim_common/src/crowd_simulator_common.cpp
...robot_sim_gazebo_plugins/src/TeleportDispenser.cpp
... and 188 more

[ddengster]

tested with use_tpe:=1 and without, in the test_ackman and office environments, it looks fine to me!