Nonholonomic slotcar turning fixes, add turning multiplier offset parameter, rename compute_ds variables

[ddengster]

Bug fix

Fixed bugs

• The nonholonomic slotcar plugin had a problem with it's turning due to the computation of the turn delta, this PR fixes it via using the arcosine of the dot product.

• Another problem is the decceleration applied to velocities in compute_ds, making our slotcar enter turns from 0 velocity and in turn making the turn look weird. To fix this we added target_velocity as a targeted final velocity as target_s reaches 0.

• Added _turning_right_angle_mul_offset as an added multiplier applied to the turning radius for right-angled turns within 20 degrees. Eg. A value of 0.5 for _turning_right_angle_mul_offset makes for a multiplier 1.5 when is applied to a 90 degree turning radius, and linearly downscales to 1.0 for a 70 degree turning radius. This compensates for turning acc/deccelerations making turns 'overshoot'.

• Corrects up some variables named velocities when they were in fact displacements.

• Removes unneeded variables in various spots

In conjunction with open-rmf/rmf_demos#82

[codecov]

Codecov Report

Merging #41 (d55cfac) into main (ebc4492) will not change coverage.
The diff coverage is n/a.

@@          Coverage Diff          @@
##            main     #41   +/-   ##
=====================================
  Coverage   0.00%   0.00%           
=====================================
  Files        138     138           
  Lines      12873   12849   -24     
=====================================
+ Misses     12873   12849   -24     

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

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

Impacted Files	Coverage Δ
...bot_sim_ignition_plugins/src/TeleportDispenser.cpp
..._sim_common/include/rmf_robot_sim_common/utils.hpp
...lding_sim_ignition_plugins/src/crowd_simulator.hpp
...on/rmf_robot_sim_ignition_plugins/src/readonly.cpp
...building_sim_common/src/crowd_simulator_common.cpp
...lding_sim_ignition_plugins/src/crowd_simulator.hpp
...lation/rmf_building_sim_common/src/lift_common.cpp
...ion/rmf_building_sim_ignition_plugins/src/door.cpp
...lding_sim_ignition_plugins/src/crowd_simulator.hpp
..._sim_common/include/rmf_robot_sim_common/utils.hpp
... and 254 more

[luca-della-vedova]

Nit, there was some discussion here about the compute_ds and compute_desired_rate_of_change being basically the same function, can we take the chance to remove one of the two?

[ddengster]

There's a line that differs between functions by double the divisor at https://github.com/open-rmf/rmf_simulation/pull/41/files/d55cfacfe9310deef0b6012e1a81ab2c7bb51394#diff-3bd4615902d61050ebeb9f584b212fc5d5ef07c1eed12cb5fa7814dbed7d59a4R41
but you're right let me remove it and test it with the other robots

[ddengster]

change delayed, will have to tweak the turning_right_angle_mul_offset values again #42

[luca-della-vedova]

Works great! Just a minor comment.
I guess for the future we might want to have a different parameter for the velocity in bends to be different (smaller) from the velocity in a straight path, but we can address that in the future.