Fix velocity control for slotcar in Ignition TPE

[luca-della-vedova]

Bug fix

Fixed bug

Slotcar in Ignition was relying on an incorrect behavior for velocity control. Specifically it was reading JointVelocityCmd to estimate the current robot's velocity, however that component is meant as a write only and the behavior of it being read back to the last command was actually not intended, and it has been fixed with it now always being read as 0.

Fix applied

Now slotcar reads the velocity from the model's canonical link and uses it for velocity control, unfortunately when using dartsim as a physics engine the robot's friction slows down the robots a lot and they barely move, however this PR fixes behavior under TPE.

To test:
ros2 launch rmf_demos_ign office.launch.xml use_tpe:=1

Robots will move after the PR but will be stuck before. Note that doors in TPE don't work since they require an old PR to be picked up again and merged.

Requires ign-physics #289 so it should be tested with a source build of Ignition with the branch above (until it gets merged)

[codecov]

Codecov Report

Merging #40 (8d336a0) into main (fc7fb26) will not change coverage.
The diff coverage is n/a.

@@          Coverage Diff          @@
##            main     #40   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files         72      72           
  Lines       6490    6510   +20     
=====================================
- Misses      6490    6510   +20     

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

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

Impacted Files	Coverage Δ
...tion/rmf_robot_sim_common/src/dispenser_common.cpp
...ation/rmf_robot_sim_gazebo_plugins/src/slotcar.cpp
...ation/rmf_building_sim_gazebo_plugins/src/door.cpp
...building_sim_common/src/crowd_simulator_common.cpp
...on_plugins/src/toggle_charging/toggle_charging.cpp
...bot_sim_ignition_plugins/src/TeleportDispenser.cpp
...on/rmf_robot_sim_ignition_plugins/src/readonly.cpp
...on/rmf_robot_sim_ignition_plugins/src/readonly.cpp
..._building_sim_gazebo_plugins/src/toggle_floors.cpp
..._robot_sim_gazebo_plugins/src/TeleportIngestor.cpp
... and 122 more

[ddengster]

Just checked with the ignition-physics PR, our robots* are moving again! Thanks!

[luca-della-vedova]

This fix still doesn't work for dartsim, might have to revert to an "open loop" style of control, closing for now.