Nav2 Docking: Optional Collision Checking on undocking / docking

[SteveMacenski]

Should be straight forward: check the future trajectory from the graceful controller for collisions, ignoring within some tolerance of the dock itself (since we're making or in contact there). We can use the costmap collision topic object from the costmap package analog to the Behavior Server's use.