Discount the scaling of future footprints when scoring a trajectory

[siferati]

Description

The obstacle cost critic scales the footprint of the robot when scoring the trajectory.
And uses this scaled footprint to check for collisions at each trajectory point.
This means that DWA will start slowing down when there is an obstacle touching the scaled footprint in the last point of the trajectory.

This PR adds a discount factor to apply to the scaling of the future footprints.
This works similarly to typical RL algorithms:

scaling(t₀) = scale
scaling(t₁) = discount_factor * scale
scaling(t₂) = discount_factor * discount_factor * scale
...
scaling(tₙ) = discount_factorⁿ * scale

As a bonus also added visualization for all of the future footprints.
This should help us tune the parameter and understand when DWA starts braking early.
Future footprints are under a different namespace, such that we can disable the noise and visualize only the current one.

By default the parameter is set to 1.0, meaning it's the same behavior as current devel.

Demo

2023-11-15.15-03-17.mp4

[renan028]

please add the link of suggestions to the task about DWA optimizations

[ChristofDubs]

Nice visualization / video.
I think the current implementation is fine (simple); few more comments based the video:

• maybe it would be easier to tune if rather than a discount factor, one could instead e.g. specify the reduction factor at the end? Or make the discount factor such that it applies to how much the next trajectory's point's footprint extends over the previous one (because in the video, with factor 0.8, basically all the future footprints just were in the initial one, which is not obvious by looking at the number 0.8)
• for guiding the robot better towards its future path, it might help if only the sideward inflation is reduced?
• on another note, it seems like we could massively increase the sim_granularity without much loss in precision? I.e. could we make dwa use half the CPU by just doubling sim_granularity? (note: addressing the first point would e.g. decouple tuning sim_granularity; right now, doubling sim_granularity would mean you have to set discount factor to its square to retain the same behavior)