Current logic to slow down vehicles too conservative for robots with very asymmetric footprint

[cesar-lopez-mar]

Currently the robot slows down based on values provided by the costmap2d. However, costmap2d fills its values based on a logic of robots with a (very close to) circular footprint robots. This makes that for rectangular robots, the robots would slow down a lot more when passing next to an obstacle than when it is approached with the corner. This can hinder the performance in environments with a lot of objects around like warehouses.